1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_trajectory_planner.cpp
7//
8// Code generated for Simulink model 'cartesian_trajectory_planner'.
9//
10// Model version : 1.129
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Wed May 20 16:41:34 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_trajectory_planner.h"
20#include "cartesian_trajectory_planner_private.h"
21
22// Block signals (default storage)
23B_cartesian_trajectory_planne_T cartesian_trajectory_planner_B;
24
25// Block states (default storage)
26DW_cartesian_trajectory_plann_T cartesian_trajectory_planner_DW;
27
28// Real-time model
29RT_MODEL_cartesian_trajectory_T cartesian_trajectory_planner_M_ =
30 RT_MODEL_cartesian_trajectory_T();
31RT_MODEL_cartesian_trajectory_T *const cartesian_trajectory_planner_M =
32 &cartesian_trajectory_planner_M_;
33
34// Forward declaration for local functions
35static real_T cartesian_trajectory_pla_norm_l(const real_T x[3]);
36static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
37 **pEmxArray, int32_T numDimensions);
38static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
39 *emxArray, int32_T oldNumel);
40static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
41 **pEmxArray, int32_T numDimensions);
42static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
43 *emxArray, int32_T oldNumel);
44static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
45 **pEmxArray);
46static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
47 **pEmxArray);
48static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
49 f_robotics_manip_internal_IKE_T *iobj_0);
50static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
51 **pEmxArray, int32_T numDimensions);
52static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
53 const real_T tform[16], const real_T weights[6]);
54static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
55 real_T Q[6]);
56static boolean_T cartesian_trajectory_pla_strcmp(const
57 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b);
58static void rigidBodyJoint_get_JointAxis_as(const
59 c_rigidBodyJoint_cartesian_as_T *obj, real_T ax[3]);
60static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
61 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
62 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
63static void rigidBodyJoint_transformBodyT_a(const
64 c_rigidBodyJoint_cartesian_as_T *obj, const real_T q_data[], const int32_T
65 *q_size, real_T T[16]);
66static void rigidBodyJoint_transformBodyToP(const
67 c_rigidBodyJoint_cartesian_as_T *obj, real_T T[16]);
68static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
69 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
70 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac);
71static creal_T cartesian_trajectory_plann_sqrt(const creal_T x);
72static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
73 int32_T ix0);
74static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
75 int32_T ix0, const real_T y[9], int32_T iy0);
76static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
77 const real_T y[9], int32_T iy0, real_T b_y[9]);
78static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0);
79static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
80 [9], int32_T ix0, real_T y[3], int32_T iy0);
81static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
82 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
83static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
84 int32_T iy0, real_T b_x[9]);
85static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
86 real_T *b_b, real_T *c, real_T *s);
87static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
88 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
89static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
90 real_T s[3], real_T V[9]);
91static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4]);
92static void cartesian_IKHelpers_computeCost(const real_T x[6],
93 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
94 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args);
95static void cartesian_trajectory_planne_eye(real_T b_I[36]);
96static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
97 **pEmxArray, int32_T numDimensions);
98static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
99 **pEmxArray, int32_T numDimensions);
100static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
101 *emxArray, int32_T oldNumel);
102static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
103 *emxArray, int32_T oldNumel);
104static real_T cartesian_trajectory_pla_norm_a(const real_T x[6]);
105static real_T SystemTimeProvider_getElapsedTi(const
106 f_robotics_core_internal_Syst_T *obj);
107static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
108 emxArray_real_T_cartesian_tra_T *x, int32_T ix0);
109static void cartesian_trajectory_pla_qrpf_a(const
110 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
111 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
112 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
113 *b_jpvt);
114static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
115 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
116 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
117 int32_T *info);
118static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
119 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
120 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
121 emxArray_real_T_cartesian_tra_T *b_B);
122static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
123 **pEmxArray);
124static void cartesian_trajectory_p_mldivide(const
125 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
126 emxArray_real_T_cartesian_tra_T *Y);
127static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
128 **pEmxArray);
129static boolean_T DampedBFGSwGradientProjection_a(const
130 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
131 emxArray_real_T_cartesian_tra_T *alpha);
132static void cartesian_trajectory_planne_inv(const
133 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y);
134static void cartesian_trajectory_plann_diag(const
135 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d);
136static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x);
137static boolean_T cartesian_trajectory_planne_any(const
138 emxArray_boolean_T_cartesian__T *x);
139static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36]);
140static boolean_T DampedBFGSwGradientProjectio_as(const
141 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);
142static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
143 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
144 *iter);
145static void cartesian_trajectory_p_isfinite(const
146 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b);
147static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2]);
148static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625]);
149static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625]);
150static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625]);
151static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
152 emxArray_real_T_cartesian_tra_T *r);
153static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
154 b_state[625], real_T *r);
155static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
156 emxArray_real_T_cartesian_tra_T *r);
157static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
158 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
159 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
160 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
161 solutionInfo_Status_size[2]);
162static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
163 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
164 real_T QSol[6]);
165static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
166 *emxArray, int32_T oldNumel);
167static void ca_rigidBodyJoint_get_JointAxis(const
168 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3]);
169static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
170 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
171static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
172 **pEmxArray);
173static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_Rig_T *obj,
174 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac);
175static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
176 *obj, real_T ax[3]);
177static void RigidBodyTree_forwardKinemati_a(p_robotics_manip_internal_R_a_T *obj,
178 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
179static void matlabCodegenHandle_matla_astwh(ros_slros_internal_block_Subs_T *obj);
180static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj);
181static void matlabCodegenHandle_matlabC_ast(robotics_slmanip_internal__as_T *obj);
182static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
183static void cartesian__SystemCore_delete_as(b_inverseKinematics_cartesian_T *obj);
184static void matlabCodegenHandle_matlabCo_as(b_inverseKinematics_cartesian_T *obj);
185static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_as_T
186 *pStruct);
187static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
188 *pStruct);
189static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
190 *pStruct);
191static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
192 *pStruct);
193static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal__as_T
194 *pStruct);
195static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
196 *pStruct);
197static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
198 *pStruct);
199static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
200 *pStruct);
201static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
202 *pStruct);
203static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
204 *pStruct);
205static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
206 *pStruct);
207static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
208 *pStruct);
209static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
210 *pStruct);
211static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
212 *pStruct);
213static void emxFreeStruct_c_rigidBodyJoint2(c_rigidBodyJoint_cartesian__a_T
214 *pStruct);
215static void emxFreeStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
216 *pStruct);
217static void emxFreeStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
218 *pStruct);
219static void emxFreeStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
220 *pStruct);
221static void emxFreeStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
222 *pStruct);
223static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_Publ_T *obj);
224static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_as_T
225 *pStruct);
226static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
227 *pStruct);
228static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
229 *pStruct);
230static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
231 *pStruct);
232static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal__as_T
233 *pStruct);
234static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
235 *pStruct);
236static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
237 *pStruct);
238static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
239 *pStruct);
240static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
241 *pStruct);
242static void cartesia_twister_state_vector_a(uint32_T mt[625]);
243static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625]);
244static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
245 (v_robotics_manip_internal_Rig_T *obj);
246static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
247 (v_robotics_manip_internal_Rig_T *obj);
248static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
249 (v_robotics_manip_internal_Rig_T *obj);
250static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
251 (v_robotics_manip_internal_Rig_T *obj);
252static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
253 (v_robotics_manip_internal_Rig_T *obj);
254static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
255 (v_robotics_manip_internal_Rig_T *obj);
256static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
257 (v_robotics_manip_internal_Rig_T *obj);
258static y_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
259 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
260 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
261 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
262 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
263 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
264 v_robotics_manip_internal_Rig_T *iobj_7);
265static void cartesian_trajectory_plann_rand(real_T r[5]);
266static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h
267 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
268static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h2
269 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
270static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_m
271 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
272static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_o
273 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
274static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_d
275 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
276static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
277 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
278 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
279 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
280 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
281 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_as_T
282 *iobj_7, c_rigidBodyJoint_cartesian_as_T *iobj_8,
283 c_rigidBodyJoint_cartesian_as_T *iobj_9, c_rigidBodyJoint_cartesian_as_T
284 *iobj_10, c_rigidBodyJoint_cartesian_as_T *iobj_11,
285 c_rigidBodyJoint_cartesian_as_T *iobj_12, c_rigidBodyJoint_cartesian_as_T
286 *iobj_13, c_rigidBodyJoint_cartesian_as_T *iobj_14,
287 w_robotics_manip_internal_Rig_T *iobj_15);
288static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ast
289 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
290 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
291 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
292 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
293 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
294 c_rigidBodyJoint_cartesian_as_T *iobj_7, c_rigidBodyJoint_cartesian_as_T
295 *iobj_8, c_rigidBodyJoint_cartesian_as_T *iobj_9,
296 c_rigidBodyJoint_cartesian_as_T *iobj_10, c_rigidBodyJoint_cartesian_as_T
297 *iobj_11, c_rigidBodyJoint_cartesian_as_T *iobj_12,
298 c_rigidBodyJoint_cartesian_as_T *iobj_13, c_rigidBodyJoint_cartesian_as_T
299 *iobj_14, c_rigidBodyJoint_cartesian_as_T *iobj_15,
300 w_robotics_manip_internal_Rig_T *iobj_16);
301static c_rigidBodyJoint_cartesian_as_T *c_rigidBodyJoint_rigidBodyJoint
302 (c_rigidBodyJoint_cartesian_as_T *obj, const emxArray_char_T_cartesian_tra_T
303 *jname, const emxArray_char_T_cartesian_tra_T *jtype);
304static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
305 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0,
306 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
307 *iobj_2);
308static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
309 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
310 *parentName, c_rigidBodyJoint_cartesian_as_T *iobj_0,
311 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
312 *iobj_2);
313static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
314 y_robotics_manip_internal_Rig_T *rigidbodytree,
315 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
316 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
317 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
318 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
319 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
320 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
321 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
322 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
323 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
324 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
325 c_rigidBodyJoint_cartesian_as_T *iobj_15, c_rigidBodyJoint_cartesian_as_T
326 *iobj_16, c_rigidBodyJoint_cartesian_as_T *iobj_17,
327 c_rigidBodyJoint_cartesian_as_T *iobj_18, c_rigidBodyJoint_cartesian_as_T
328 *iobj_19, c_rigidBodyJoint_cartesian_as_T *iobj_20,
329 c_rigidBodyJoint_cartesian_as_T *iobj_21, c_rigidBodyJoint_cartesian_as_T
330 *iobj_22, c_rigidBodyJoint_cartesian_as_T *iobj_23,
331 c_rigidBodyJoint_cartesian_as_T *iobj_24, c_rigidBodyJoint_cartesian_as_T
332 *iobj_25, c_rigidBodyJoint_cartesian_as_T *iobj_26,
333 c_rigidBodyJoint_cartesian_as_T *iobj_27, c_rigidBodyJoint_cartesian_as_T
334 *iobj_28, c_rigidBodyJoint_cartesian_as_T *iobj_29,
335 c_rigidBodyJoint_cartesian_as_T *iobj_30, c_rigidBodyJoint_cartesian_as_T
336 *iobj_31, c_rigidBodyJoint_cartesian_as_T *iobj_32,
337 c_rigidBodyJoint_cartesian_as_T *iobj_33, c_rigidBodyJoint_cartesian_as_T
338 *iobj_34, c_rigidBodyJoint_cartesian_as_T *iobj_35,
339 c_rigidBodyJoint_cartesian_as_T *iobj_36, c_rigidBodyJoint_cartesian_as_T
340 *iobj_37, c_rigidBodyJoint_cartesian_as_T *iobj_38,
341 c_rigidBodyJoint_cartesian_as_T *iobj_39, w_robotics_manip_internal_Rig_T
342 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);
343static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
344 (h_robotics_core_internal_Damp_T *obj);
345static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
346 *pStruct);
347static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
348 *pStruct);
349static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
350 *pStruct);
351static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
352 *pStruct);
353static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
354 *pStruct);
355static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
356 (n_robotics_manip_internal_Rig_T *obj);
357static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_a
358 (n_robotics_manip_internal_Rig_T *obj);
359static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_as
360 (n_robotics_manip_internal_Rig_T *obj);
361static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_ast
362 (n_robotics_manip_internal_Rig_T *obj);
363static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_astw
364 (n_robotics_manip_internal_Rig_T *obj);
365static n_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_astwh
366 (n_robotics_manip_internal_Rig_T *obj);
367static n_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_astwhq
368 (n_robotics_manip_internal_Rig_T *obj);
369static n_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_astwhqf
370 (n_robotics_manip_internal_Rig_T *obj);
371static o_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_astwhqf2
372 (o_robotics_manip_internal_Rig_T *obj);
373static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
374 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
375 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
376 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
377 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
378 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
379 n_robotics_manip_internal_Rig_T *iobj_7);
380static void emxInitStruct_c_rigidBodyJoint2(c_rigidBodyJoint_cartesian__a_T
381 *pStruct);
382static void emxInitStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
383 *pStruct);
384static void emxInitStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
385 *pStruct);
386static void emxInitStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
387 *pStruct);
388static void emxInitStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
389 *pStruct);
390static n_robotics_manip_internal_R_a_T *c_RigidBody_RigidBody_astwhqf2a
391 (n_robotics_manip_internal_R_a_T *obj);
392static n_robotics_manip_internal_R_a_T *RigidBody_RigidBody_astwhqf2az
393 (n_robotics_manip_internal_R_a_T *obj);
394static n_robotics_manip_internal_R_a_T *RigidBody_RigidBody_astwhqf2azt
395 (n_robotics_manip_internal_R_a_T *obj);
396static n_robotics_manip_internal_R_a_T *RigidBody_RigidBod_astwhqf2aztt
397 (n_robotics_manip_internal_R_a_T *obj);
398static n_robotics_manip_internal_R_a_T *RigidBody_RigidBo_astwhqf2azttx
399 (n_robotics_manip_internal_R_a_T *obj);
400static p_robotics_manip_internal_R_a_T *c_RigidBodyTree_RigidBodyTree_a
401 (p_robotics_manip_internal_R_a_T *obj, n_robotics_manip_internal_R_a_T *iobj_0,
402 n_robotics_manip_internal_R_a_T *iobj_1, n_robotics_manip_internal_R_a_T
403 *iobj_2, n_robotics_manip_internal_R_a_T *iobj_3,
404 n_robotics_manip_internal_R_a_T *iobj_4, n_robotics_manip_internal_R_a_T
405 *iobj_5, n_robotics_manip_internal_R_a_T *iobj_6,
406 n_robotics_manip_internal_R_a_T *iobj_7);
407int32_T div_s32_floor(int32_T numerator, int32_T denominator)
408{
409 int32_T quotient;
410 uint32_T absNumerator;
411 uint32_T absDenominator;
412 uint32_T tempAbsQuotient;
413 boolean_T quotientNeedsNegation;
414 if (denominator == 0) {
415 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
416
417 // Divide by zero handler
418 } else {
419 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
420 static_cast<uint32_T>(numerator);
421 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
422 static_cast<uint32_T>(denominator);
423 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
424 tempAbsQuotient = absNumerator / absDenominator;
425 if (quotientNeedsNegation) {
426 absNumerator %= absDenominator;
427 if (absNumerator > 0U) {
428 tempAbsQuotient++;
429 }
430 }
431
432 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
433 static_cast<int32_T>(tempAbsQuotient);
434 }
435
436 return quotient;
437}
438
439// Function for MATLAB Function: '<Root>/Traslazione '
440static real_T cartesian_trajectory_pla_norm_l(const real_T x[3])
441{
442 real_T y;
443 real_T scale;
444 real_T absxk;
445 real_T t;
446 scale = 3.3121686421112381E-170;
447 absxk = fabs(x[0]);
448 if (absxk > 3.3121686421112381E-170) {
449 y = 1.0;
450 scale = absxk;
451 } else {
452 t = absxk / 3.3121686421112381E-170;
453 y = t * t;
454 }
455
456 absxk = fabs(x[1]);
457 if (absxk > scale) {
458 t = scale / absxk;
459 y = y * t * t + 1.0;
460 scale = absxk;
461 } else {
462 t = absxk / scale;
463 y += t * t;
464 }
465
466 absxk = fabs(x[2]);
467 if (absxk > scale) {
468 t = scale / absxk;
469 y = y * t * t + 1.0;
470 scale = absxk;
471 } else {
472 t = absxk / scale;
473 y += t * t;
474 }
475
476 return scale * sqrt(y);
477}
478
479static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
480 **pEmxArray, int32_T numDimensions)
481{
482 emxArray_real_T_cartesian_tra_T *emxArray;
483 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)malloc(sizeof
484 (emxArray_real_T_cartesian_tra_T));
485 emxArray = *pEmxArray;
486 emxArray->data = (real_T *)NULL;
487 emxArray->numDimensions = numDimensions;
488 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
489 emxArray->allocatedSize = 0;
490 emxArray->canFreeData = true;
491 for (cartesian_trajectory_planner_B.i_k = 0;
492 cartesian_trajectory_planner_B.i_k < numDimensions;
493 cartesian_trajectory_planner_B.i_k++) {
494 emxArray->size[cartesian_trajectory_planner_B.i_k] = 0;
495 }
496}
497
498static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
499 *emxArray, int32_T oldNumel)
500{
501 void *newData;
502 if (oldNumel < 0) {
503 oldNumel = 0;
504 }
505
506 cartesian_trajectory_planner_B.newNumel_i = 1;
507 for (cartesian_trajectory_planner_B.i_e = 0;
508 cartesian_trajectory_planner_B.i_e < emxArray->numDimensions;
509 cartesian_trajectory_planner_B.i_e++) {
510 cartesian_trajectory_planner_B.newNumel_i *= emxArray->
511 size[cartesian_trajectory_planner_B.i_e];
512 }
513
514 if (cartesian_trajectory_planner_B.newNumel_i > emxArray->allocatedSize) {
515 cartesian_trajectory_planner_B.i_e = emxArray->allocatedSize;
516 if (cartesian_trajectory_planner_B.i_e < 16) {
517 cartesian_trajectory_planner_B.i_e = 16;
518 }
519
520 while (cartesian_trajectory_planner_B.i_e <
521 cartesian_trajectory_planner_B.newNumel_i) {
522 if (cartesian_trajectory_planner_B.i_e > 1073741823) {
523 cartesian_trajectory_planner_B.i_e = MAX_int32_T;
524 } else {
525 cartesian_trajectory_planner_B.i_e <<= 1;
526 }
527 }
528
529 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_e),
530 sizeof(real_T));
531 if (emxArray->data != NULL) {
532 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
533 if (emxArray->canFreeData) {
534 free(emxArray->data);
535 }
536 }
537
538 emxArray->data = (real_T *)newData;
539 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_e;
540 emxArray->canFreeData = true;
541 }
542}
543
544static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
545 **pEmxArray, int32_T numDimensions)
546{
547 emxArray_char_T_cartesian_tra_T *emxArray;
548 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)malloc(sizeof
549 (emxArray_char_T_cartesian_tra_T));
550 emxArray = *pEmxArray;
551 emxArray->data = (char_T *)NULL;
552 emxArray->numDimensions = numDimensions;
553 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
554 emxArray->allocatedSize = 0;
555 emxArray->canFreeData = true;
556 for (cartesian_trajectory_planner_B.i_j = 0;
557 cartesian_trajectory_planner_B.i_j < numDimensions;
558 cartesian_trajectory_planner_B.i_j++) {
559 emxArray->size[cartesian_trajectory_planner_B.i_j] = 0;
560 }
561}
562
563static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
564 *emxArray, int32_T oldNumel)
565{
566 void *newData;
567 if (oldNumel < 0) {
568 oldNumel = 0;
569 }
570
571 cartesian_trajectory_planner_B.newNumel = 1;
572 for (cartesian_trajectory_planner_B.i_l = 0;
573 cartesian_trajectory_planner_B.i_l < emxArray->numDimensions;
574 cartesian_trajectory_planner_B.i_l++) {
575 cartesian_trajectory_planner_B.newNumel *= emxArray->
576 size[cartesian_trajectory_planner_B.i_l];
577 }
578
579 if (cartesian_trajectory_planner_B.newNumel > emxArray->allocatedSize) {
580 cartesian_trajectory_planner_B.i_l = emxArray->allocatedSize;
581 if (cartesian_trajectory_planner_B.i_l < 16) {
582 cartesian_trajectory_planner_B.i_l = 16;
583 }
584
585 while (cartesian_trajectory_planner_B.i_l <
586 cartesian_trajectory_planner_B.newNumel) {
587 if (cartesian_trajectory_planner_B.i_l > 1073741823) {
588 cartesian_trajectory_planner_B.i_l = MAX_int32_T;
589 } else {
590 cartesian_trajectory_planner_B.i_l <<= 1;
591 }
592 }
593
594 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_l),
595 sizeof(char_T));
596 if (emxArray->data != NULL) {
597 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
598 if (emxArray->canFreeData) {
599 free(emxArray->data);
600 }
601 }
602
603 emxArray->data = (char_T *)newData;
604 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_l;
605 emxArray->canFreeData = true;
606 }
607}
608
609static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
610 **pEmxArray)
611{
612 if (*pEmxArray != (emxArray_real_T_cartesian_tra_T *)NULL) {
613 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
614 free((*pEmxArray)->data);
615 }
616
617 free((*pEmxArray)->size);
618 free(*pEmxArray);
619 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)NULL;
620 }
621}
622
623static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
624 **pEmxArray)
625{
626 if (*pEmxArray != (emxArray_char_T_cartesian_tra_T *)NULL) {
627 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
628 free((*pEmxArray)->data);
629 }
630
631 free((*pEmxArray)->size);
632 free(*pEmxArray);
633 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)NULL;
634 }
635}
636
637static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
638 f_robotics_manip_internal_IKE_T *iobj_0)
639{
640 real_T n;
641 emxArray_real_T_cartesian_tra_T *A;
642 emxArray_real_T_cartesian_tra_T *b;
643 real_T m;
644 c_rigidBodyJoint_cartesian_as_T *joint;
645 real_T pnum;
646 int32_T d;
647 int32_T b_i;
648 emxArray_real_T_cartesian_tra_T *e;
649 int32_T j;
650 int32_T p;
651 emxArray_real_T_cartesian_tra_T *s;
652 x_robotics_manip_internal_Rig_T *obj_0;
653 w_robotics_manip_internal_Rig_T *body;
654 int32_T c;
655 boolean_T b_bool;
656 emxArray_char_T_cartesian_tra_T *a;
657 int32_T b_kstr;
658 char_T b_0[5];
659 int8_T b_I[16];
660 int32_T m_0;
661 real_T t;
662 real_T pnum_0;
663 int32_T loop_ub;
664 real_T tmp;
665 real_T w;
666 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
667
668 int32_T exitg1;
669 cartesian_trajec_emxInit_real_T(&A, 2);
670 n = obj->RigidBodyTreeInternal->PositionNumber;
671 b_kstr = A->size[0] * A->size[1];
672 c = static_cast<int32_T>(n);
673 A->size[0] = c;
674 loop_ub = static_cast<int32_T>(2.0 * n);
675 A->size[1] = loop_ub;
676 cartes_emxEnsureCapacity_real_T(A, b_kstr);
677 m_0 = loop_ub * c - 1;
678 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
679 A->data[b_kstr] = 0.0;
680 }
681
682 cartesian_trajec_emxInit_real_T(&b, 1);
683 b_kstr = b->size[0];
684 b->size[0] = loop_ub;
685 cartes_emxEnsureCapacity_real_T(b, b_kstr);
686 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
687 b->data[b_kstr] = 0.0;
688 }
689
690 n = 1.0;
691 m = 1.0;
692 pnum = obj->RigidBodyTreeInternal->NumBodies;
693 d = static_cast<int32_T>(pnum) - 1;
694 cartesian_trajec_emxInit_real_T(&e, 2);
695 cartesian_trajec_emxInit_real_T(&s, 2);
696 cartesian_trajec_emxInit_char_T(&a, 2);
697 if (0 <= d) {
698 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
699 b_0[b_kstr] = tmp_0[b_kstr];
700 }
701 }
702
703 for (b_i = 0; b_i <= d; b_i++) {
704 body = obj->RigidBodyTreeInternal->Bodies[b_i];
705 joint = body->JointInternal;
706 pnum = joint->PositionNumber;
707 b_kstr = a->size[0] * a->size[1];
708 a->size[0] = 1;
709 a->size[1] = joint->Type->size[1];
710 cartes_emxEnsureCapacity_char_T(a, b_kstr);
711 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
712 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
713 a->data[b_kstr] = joint->Type->data[b_kstr];
714 }
715
716 b_bool = false;
717 if (a->size[1] == 5) {
718 b_kstr = 1;
719 do {
720 exitg1 = 0;
721 if (b_kstr - 1 < 5) {
722 loop_ub = b_kstr - 1;
723 if (a->data[loop_ub] != b_0[loop_ub]) {
724 exitg1 = 1;
725 } else {
726 b_kstr++;
727 }
728 } else {
729 b_bool = true;
730 exitg1 = 1;
731 }
732 } while (exitg1 == 0);
733 }
734
735 if (!b_bool) {
736 tmp = (n + pnum) - 1.0;
737 if (n > tmp) {
738 j = 0;
739 } else {
740 j = static_cast<int32_T>(n) - 1;
741 }
742
743 w = m + pnum;
744 if (m > w - 1.0) {
745 p = 0;
746 } else {
747 p = static_cast<int32_T>(m) - 1;
748 }
749
750 if (pnum < 0.0) {
751 t = 0.0;
752 pnum_0 = 0.0;
753 } else {
754 t = pnum;
755 pnum_0 = pnum;
756 }
757
758 m_0 = static_cast<int32_T>(pnum_0) - 1;
759 b_kstr = s->size[0] * s->size[1];
760 c = static_cast<int32_T>(t);
761 s->size[0] = c;
762 s->size[1] = c;
763 cartes_emxEnsureCapacity_real_T(s, b_kstr);
764 loop_ub = c * c - 1;
765 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
766 s->data[b_kstr] = 0.0;
767 }
768
769 if (c > 0) {
770 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
771 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
772 }
773 }
774
775 loop_ub = s->size[1];
776 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
777 m_0 = s->size[0];
778 for (c = 0; c < m_0; c++) {
779 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
780 b_kstr + c];
781 }
782 }
783
784 if (n > tmp) {
785 j = 0;
786 } else {
787 j = static_cast<int32_T>(n) - 1;
788 }
789
790 tmp = 2.0 * pnum + m;
791 if (w > tmp - 1.0) {
792 p = 0;
793 } else {
794 p = static_cast<int32_T>(w) - 1;
795 }
796
797 if (pnum < 0.0) {
798 t = 0.0;
799 pnum_0 = 0.0;
800 } else {
801 t = pnum;
802 pnum_0 = pnum;
803 }
804
805 m_0 = static_cast<int32_T>(pnum_0) - 1;
806 b_kstr = s->size[0] * s->size[1];
807 c = static_cast<int32_T>(t);
808 s->size[0] = c;
809 s->size[1] = c;
810 cartes_emxEnsureCapacity_real_T(s, b_kstr);
811 loop_ub = c * c - 1;
812 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
813 s->data[b_kstr] = 0.0;
814 }
815
816 if (c > 0) {
817 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
818 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
819 }
820 }
821
822 loop_ub = s->size[1];
823 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
824 m_0 = s->size[0];
825 for (c = 0; c < m_0; c++) {
826 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
827 b_kstr + c];
828 }
829 }
830
831 b_kstr = e->size[0] * e->size[1];
832 e->size[0] = joint->PositionLimitsInternal->size[0];
833 e->size[1] = 2;
834 cartes_emxEnsureCapacity_real_T(e, b_kstr);
835 loop_ub = joint->PositionLimitsInternal->size[0] *
836 joint->PositionLimitsInternal->size[1] - 1;
837 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
838 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
839 }
840
841 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
842 b_kstr = e->size[0] * e->size[1];
843 e->size[0] = joint->PositionLimitsInternal->size[0];
844 e->size[1] = 2;
845 cartes_emxEnsureCapacity_real_T(e, b_kstr);
846 loop_ub = joint->PositionLimitsInternal->size[0] *
847 joint->PositionLimitsInternal->size[1] - 1;
848 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
849 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
850 }
851
852 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
853 m = tmp;
854 }
855
856 n += pnum;
857 }
858
859 cartesian_trajec_emxFree_real_T(&s);
860 b_kstr = A->size[0] * A->size[1];
861 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
862 ConstraintMatrix->size[1];
863 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
864 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
865 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);
866 loop_ub = b_kstr - 1;
867 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
868 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
869 }
870
871 cartesian_trajec_emxFree_real_T(&A);
872 b_kstr = obj->Solver->ConstraintBound->size[0];
873 obj->Solver->ConstraintBound->size[0] = b->size[0];
874 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);
875 loop_ub = b->size[0];
876 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
877 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
878 }
879
880 obj_0 = obj->RigidBodyTreeInternal;
881 b_kstr = e->size[0] * e->size[1];
882 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
883 e->size[1] = 2;
884 cartes_emxEnsureCapacity_real_T(e, b_kstr);
885 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
886 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
887 e->data[b_kstr] = 0.0;
888 }
889
890 n = 1.0;
891 m = obj_0->NumBodies;
892 c = static_cast<int32_T>(m) - 1;
893 if (0 <= c) {
894 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
895 b_0[b_kstr] = tmp_0[b_kstr];
896 }
897 }
898
899 for (b_i = 0; b_i <= c; b_i++) {
900 body = obj_0->Bodies[b_i];
901 b_kstr = a->size[0] * a->size[1];
902 a->size[0] = 1;
903 a->size[1] = body->JointInternal->Type->size[1];
904 cartes_emxEnsureCapacity_char_T(a, b_kstr);
905 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
906 Type->size[1] - 1;
907 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
908 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
909 }
910
911 b_bool = false;
912 if (a->size[1] == 5) {
913 b_kstr = 1;
914 do {
915 exitg1 = 0;
916 if (b_kstr - 1 < 5) {
917 loop_ub = b_kstr - 1;
918 if (a->data[loop_ub] != b_0[loop_ub]) {
919 exitg1 = 1;
920 } else {
921 b_kstr++;
922 }
923 } else {
924 b_bool = true;
925 exitg1 = 1;
926 }
927 } while (exitg1 == 0);
928 }
929
930 if (!b_bool) {
931 pnum = body->JointInternal->PositionNumber;
932 tmp = n + pnum;
933 if (n > tmp - 1.0) {
934 d = 0;
935 } else {
936 d = static_cast<int32_T>(n) - 1;
937 }
938
939 joint = body->JointInternal;
940 loop_ub = joint->PositionLimitsInternal->size[0];
941 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
942 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
943 }
944
945 loop_ub = joint->PositionLimitsInternal->size[0];
946 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
947 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
948 data[b_kstr + joint->PositionLimitsInternal->size[0]];
949 }
950
951 n = tmp;
952 }
953 }
954
955 cartesian_trajec_emxFree_char_T(&a);
956 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
957 obj->Limits->size[0] = e->size[0];
958 obj->Limits->size[1] = 2;
959 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);
960 loop_ub = e->size[0] * e->size[1] - 1;
961 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
962 obj->Limits->data[b_kstr] = e->data[b_kstr];
963 }
964
965 cartesian_trajec_emxFree_real_T(&e);
966 obj->Solver->ExtraArgs = iobj_0;
967 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
968 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
969 }
970
971 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
972 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
973 b_I[b_kstr] = 0;
974 }
975
976 b_I[0] = 1;
977 b_I[5] = 1;
978 b_I[10] = 1;
979 b_I[15] = 1;
980 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
981 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
982 }
983
984 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
985 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
986 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
987 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
988 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);
989 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
990 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
991 }
992
993 obj->Solver->ExtraArgs->CostTemp = 0.0;
994 b_kstr = b->size[0];
995 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
996 cartes_emxEnsureCapacity_real_T(b, b_kstr);
997 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
998 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
999 b->data[b_kstr] = 0.0;
1000 }
1001
1002 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
1003 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
1004 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);
1005 loop_ub = b->size[0];
1006 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1007 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
1008 }
1009
1010 cartesian_trajec_emxFree_real_T(&b);
1011}
1012
1013static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
1014 **pEmxArray, int32_T numDimensions)
1015{
1016 emxArray_f_cell_wrap_cartesia_T *emxArray;
1017 int32_T i;
1018 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
1019 (emxArray_f_cell_wrap_cartesia_T));
1020 emxArray = *pEmxArray;
1021 emxArray->data = (f_cell_wrap_cartesian_traject_T *)NULL;
1022 emxArray->numDimensions = numDimensions;
1023 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
1024 emxArray->allocatedSize = 0;
1025 emxArray->canFreeData = true;
1026 for (i = 0; i < numDimensions; i++) {
1027 emxArray->size[i] = 0;
1028 }
1029}
1030
1031static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
1032 const real_T tform[16], const real_T weights[6])
1033{
1034 real_T weightMatrix[36];
1035 f_robotics_manip_internal_IKE_T *args;
1036 int32_T b_j;
1037 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1038 'e', 'e' };
1039
1040 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
1041 for (b_j = 0; b_j < 6; b_j++) {
1042 weightMatrix[b_j + 6 * b_j] = weights[b_j];
1043 }
1044
1045 args = obj->Solver->ExtraArgs;
1046 for (b_j = 0; b_j < 36; b_j++) {
1047 args->WeightMatrix[b_j] = weightMatrix[b_j];
1048 }
1049
1050 b_j = args->BodyName->size[0] * args->BodyName->size[1];
1051 args->BodyName->size[0] = 1;
1052 args->BodyName->size[1] = 11;
1053 cartes_emxEnsureCapacity_char_T(args->BodyName, b_j);
1054 for (b_j = 0; b_j < 11; b_j++) {
1055 args->BodyName->data[b_j] = tmp[b_j];
1056 }
1057
1058 for (b_j = 0; b_j < 16; b_j++) {
1059 args->Tform[b_j] = tform[b_j];
1060 }
1061}
1062
1063static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
1064 real_T Q[6])
1065{
1066 emxArray_real_T_cartesian_tra_T *limits;
1067 boolean_T ubOK[6];
1068 boolean_T lbOK[6];
1069 real_T k;
1070 w_robotics_manip_internal_Rig_T *body;
1071 real_T pnum;
1072 int32_T c;
1073 int32_T f;
1074 int32_T ii_data[6];
1075 boolean_T b_bool;
1076 emxArray_char_T_cartesian_tra_T *a;
1077 int32_T b_kstr;
1078 char_T b[5];
1079 c_rigidBodyJoint_cartesian_as_T *obj_0;
1080 int32_T idx;
1081 int32_T loop_ub;
1082 emxArray_real_T_cartesian_tra_T *limits_0;
1083 emxArray_real_T_cartesian_tra_T *limits_1;
1084 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1085
1086 boolean_T guard1 = false;
1087 int32_T exitg1;
1088 boolean_T exitg2;
1089 cartesian_trajec_emxInit_real_T(&limits, 2);
1090 b_kstr = limits->size[0] * limits->size[1];
1091 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
1092 limits->size[1] = 2;
1093 cartes_emxEnsureCapacity_real_T(limits, b_kstr);
1094 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
1095 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1096 limits->data[b_kstr] = 0.0;
1097 }
1098
1099 k = 1.0;
1100 pnum = obj->NumBodies;
1101 c = static_cast<int32_T>(pnum) - 1;
1102 cartesian_trajec_emxInit_char_T(&a, 2);
1103 if (0 <= c) {
1104 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1105 b[b_kstr] = tmp[b_kstr];
1106 }
1107 }
1108
1109 for (idx = 0; idx <= c; idx++) {
1110 body = obj->Bodies[idx];
1111 b_kstr = a->size[0] * a->size[1];
1112 a->size[0] = 1;
1113 a->size[1] = body->JointInternal->Type->size[1];
1114 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1115 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
1116 Type->size[1] - 1;
1117 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1118 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
1119 }
1120
1121 b_bool = false;
1122 if (a->size[1] == 5) {
1123 b_kstr = 1;
1124 do {
1125 exitg1 = 0;
1126 if (b_kstr - 1 < 5) {
1127 loop_ub = b_kstr - 1;
1128 if (a->data[loop_ub] != b[loop_ub]) {
1129 exitg1 = 1;
1130 } else {
1131 b_kstr++;
1132 }
1133 } else {
1134 b_bool = true;
1135 exitg1 = 1;
1136 }
1137 } while (exitg1 == 0);
1138 }
1139
1140 if (!b_bool) {
1141 pnum = body->JointInternal->PositionNumber;
1142 pnum += k;
1143 if (k > pnum - 1.0) {
1144 f = 0;
1145 } else {
1146 f = static_cast<int32_T>(k) - 1;
1147 }
1148
1149 obj_0 = body->JointInternal;
1150 loop_ub = obj_0->PositionLimitsInternal->size[0];
1151 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1152 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
1153 }
1154
1155 loop_ub = obj_0->PositionLimitsInternal->size[0];
1156 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1157 limits->data[(f + b_kstr) + limits->size[0]] =
1158 obj_0->PositionLimitsInternal->data[b_kstr +
1159 obj_0->PositionLimitsInternal->size[0]];
1160 }
1161
1162 k = pnum;
1163 }
1164 }
1165
1166 cartesian_trajec_emxFree_char_T(&a);
1167 cartesian_trajec_emxInit_real_T(&limits_0, 1);
1168 loop_ub = limits->size[0];
1169 b_kstr = limits_0->size[0];
1170 limits_0->size[0] = loop_ub;
1171 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);
1172 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1173 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
1174 4.4408920985006262E-16;
1175 }
1176
1177 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1178 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
1179 }
1180
1181 cartesian_trajec_emxFree_real_T(&limits_0);
1182 cartesian_trajec_emxInit_real_T(&limits_1, 1);
1183 loop_ub = limits->size[0];
1184 b_kstr = limits_1->size[0];
1185 limits_1->size[0] = loop_ub;
1186 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);
1187 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1188 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
1189 }
1190
1191 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1192 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
1193 }
1194
1195 cartesian_trajec_emxFree_real_T(&limits_1);
1196 b_bool = true;
1197 idx = 0;
1198 exitg2 = false;
1199 while ((!exitg2) && (idx < 6)) {
1200 if (!ubOK[idx]) {
1201 b_bool = false;
1202 exitg2 = true;
1203 } else {
1204 idx++;
1205 }
1206 }
1207
1208 guard1 = false;
1209 if (b_bool) {
1210 b_bool = true;
1211 idx = 0;
1212 exitg2 = false;
1213 while ((!exitg2) && (idx < 6)) {
1214 if (!lbOK[idx]) {
1215 b_bool = false;
1216 exitg2 = true;
1217 } else {
1218 idx++;
1219 }
1220 }
1221
1222 if (b_bool) {
1223 } else {
1224 guard1 = true;
1225 }
1226 } else {
1227 guard1 = true;
1228 }
1229
1230 if (guard1) {
1231 idx = 0;
1232 b_kstr = 1;
1233 exitg2 = false;
1234 while ((!exitg2) && (b_kstr - 1 < 6)) {
1235 if (!ubOK[b_kstr - 1]) {
1236 idx++;
1237 ii_data[idx - 1] = b_kstr;
1238 if (idx >= 6) {
1239 exitg2 = true;
1240 } else {
1241 b_kstr++;
1242 }
1243 } else {
1244 b_kstr++;
1245 }
1246 }
1247
1248 if (1 > idx) {
1249 idx = 0;
1250 }
1251
1252 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1253 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
1254 - 1];
1255 }
1256
1257 idx = 0;
1258 b_kstr = 1;
1259 exitg2 = false;
1260 while ((!exitg2) && (b_kstr - 1 < 6)) {
1261 if (!lbOK[b_kstr - 1]) {
1262 idx++;
1263 ii_data[idx - 1] = b_kstr;
1264 if (idx >= 6) {
1265 exitg2 = true;
1266 } else {
1267 b_kstr++;
1268 }
1269 } else {
1270 b_kstr++;
1271 }
1272 }
1273
1274 if (1 > idx) {
1275 idx = 0;
1276 }
1277
1278 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1279 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
1280 }
1281 }
1282
1283 cartesian_trajec_emxFree_real_T(&limits);
1284}
1285
1286static boolean_T cartesian_trajectory_pla_strcmp(const
1287 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b)
1288{
1289 boolean_T b_bool;
1290 int32_T kstr;
1291 int32_T b_kstr;
1292 boolean_T d;
1293 int32_T exitg1;
1294 b_bool = false;
1295 d = (a->size[1] == 0);
1296 if (d && (b->size[1] == 0)) {
1297 b_bool = true;
1298 } else if (a->size[1] != b->size[1]) {
1299 } else {
1300 b_kstr = 1;
1301 do {
1302 exitg1 = 0;
1303 if (b_kstr - 1 <= b->size[1] - 1) {
1304 kstr = b_kstr - 1;
1305 if (a->data[kstr] != b->data[kstr]) {
1306 exitg1 = 1;
1307 } else {
1308 b_kstr++;
1309 }
1310 } else {
1311 b_bool = true;
1312 exitg1 = 1;
1313 }
1314 } while (exitg1 == 0);
1315 }
1316
1317 return b_bool;
1318}
1319
1320static void rigidBodyJoint_get_JointAxis_as(const
1321 c_rigidBodyJoint_cartesian_as_T *obj, real_T ax[3])
1322{
1323 emxArray_char_T_cartesian_tra_T *a;
1324 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1325
1326 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1327
1328 boolean_T guard1 = false;
1329 int32_T exitg1;
1330 cartesian_trajec_emxInit_char_T(&a, 2);
1331 cartesian_trajectory_planner_B.b_kstr_m = a->size[0] * a->size[1];
1332 a->size[0] = 1;
1333 a->size[1] = obj->Type->size[1];
1334 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_m);
1335 cartesian_trajectory_planner_B.loop_ub_g = obj->Type->size[0] * obj->
1336 Type->size[1] - 1;
1337 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
1338 cartesian_trajectory_planner_B.b_kstr_m <=
1339 cartesian_trajectory_planner_B.loop_ub_g;
1340 cartesian_trajectory_planner_B.b_kstr_m++) {
1341 a->data[cartesian_trajectory_planner_B.b_kstr_m] = obj->Type->
1342 data[cartesian_trajectory_planner_B.b_kstr_m];
1343 }
1344
1345 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
1346 cartesian_trajectory_planner_B.b_kstr_m < 8;
1347 cartesian_trajectory_planner_B.b_kstr_m++) {
1348 cartesian_trajectory_planner_B.b_j[cartesian_trajectory_planner_B.b_kstr_m] =
1349 tmp[cartesian_trajectory_planner_B.b_kstr_m];
1350 }
1351
1352 cartesian_trajectory_planner_B.b_bool_c = false;
1353 if (a->size[1] == 8) {
1354 cartesian_trajectory_planner_B.b_kstr_m = 1;
1355 do {
1356 exitg1 = 0;
1357 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 8) {
1358 cartesian_trajectory_planner_B.loop_ub_g =
1359 cartesian_trajectory_planner_B.b_kstr_m - 1;
1360 if (a->data[cartesian_trajectory_planner_B.loop_ub_g] !=
1361 cartesian_trajectory_planner_B.b_j[cartesian_trajectory_planner_B.loop_ub_g])
1362 {
1363 exitg1 = 1;
1364 } else {
1365 cartesian_trajectory_planner_B.b_kstr_m++;
1366 }
1367 } else {
1368 cartesian_trajectory_planner_B.b_bool_c = true;
1369 exitg1 = 1;
1370 }
1371 } while (exitg1 == 0);
1372 }
1373
1374 guard1 = false;
1375 if (cartesian_trajectory_planner_B.b_bool_c) {
1376 guard1 = true;
1377 } else {
1378 cartesian_trajectory_planner_B.b_kstr_m = a->size[0] * a->size[1];
1379 a->size[0] = 1;
1380 a->size[1] = obj->Type->size[1];
1381 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_m);
1382 cartesian_trajectory_planner_B.loop_ub_g = obj->Type->size[0] * obj->
1383 Type->size[1] - 1;
1384 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
1385 cartesian_trajectory_planner_B.b_kstr_m <=
1386 cartesian_trajectory_planner_B.loop_ub_g;
1387 cartesian_trajectory_planner_B.b_kstr_m++) {
1388 a->data[cartesian_trajectory_planner_B.b_kstr_m] = obj->Type->
1389 data[cartesian_trajectory_planner_B.b_kstr_m];
1390 }
1391
1392 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
1393 cartesian_trajectory_planner_B.b_kstr_m < 9;
1394 cartesian_trajectory_planner_B.b_kstr_m++) {
1395 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.b_kstr_m]
1396 = tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
1397 }
1398
1399 cartesian_trajectory_planner_B.b_bool_c = false;
1400 if (a->size[1] == 9) {
1401 cartesian_trajectory_planner_B.b_kstr_m = 1;
1402 do {
1403 exitg1 = 0;
1404 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 9) {
1405 cartesian_trajectory_planner_B.loop_ub_g =
1406 cartesian_trajectory_planner_B.b_kstr_m - 1;
1407 if (a->data[cartesian_trajectory_planner_B.loop_ub_g] !=
1408 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.loop_ub_g])
1409 {
1410 exitg1 = 1;
1411 } else {
1412 cartesian_trajectory_planner_B.b_kstr_m++;
1413 }
1414 } else {
1415 cartesian_trajectory_planner_B.b_bool_c = true;
1416 exitg1 = 1;
1417 }
1418 } while (exitg1 == 0);
1419 }
1420
1421 if (cartesian_trajectory_planner_B.b_bool_c) {
1422 guard1 = true;
1423 } else {
1424 ax[0] = (rtNaN);
1425 ax[1] = (rtNaN);
1426 ax[2] = (rtNaN);
1427 }
1428 }
1429
1430 if (guard1) {
1431 ax[0] = obj->JointAxisInternal[0];
1432 ax[1] = obj->JointAxisInternal[1];
1433 ax[2] = obj->JointAxisInternal[2];
1434 }
1435
1436 cartesian_trajec_emxFree_char_T(&a);
1437}
1438
1439static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
1440 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
1441 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
1442{
1443 y[0] = varargin_1;
1444 y[1] = varargin_2;
1445 y[2] = varargin_3;
1446 y[3] = varargin_4;
1447 y[4] = varargin_5;
1448 y[5] = varargin_6;
1449 y[6] = varargin_7;
1450 y[7] = varargin_8;
1451 y[8] = varargin_9;
1452}
1453
1454static void rigidBodyJoint_transformBodyT_a(const
1455 c_rigidBodyJoint_cartesian_as_T *obj, const real_T q_data[], const int32_T
1456 *q_size, real_T T[16])
1457{
1458 emxArray_char_T_cartesian_tra_T *switch_expression;
1459 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1460
1461 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1462
1463 int32_T exitg1;
1464 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1465 cartesian_trajectory_planner_B.b_kstr_i = switch_expression->size[0] *
1466 switch_expression->size[1];
1467 switch_expression->size[0] = 1;
1468 switch_expression->size[1] = obj->Type->size[1];
1469 cartes_emxEnsureCapacity_char_T(switch_expression,
1470 cartesian_trajectory_planner_B.b_kstr_i);
1471 cartesian_trajectory_planner_B.loop_ub_h5 = obj->Type->size[0] * obj->
1472 Type->size[1] - 1;
1473 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1474 cartesian_trajectory_planner_B.b_kstr_i <=
1475 cartesian_trajectory_planner_B.loop_ub_h5;
1476 cartesian_trajectory_planner_B.b_kstr_i++) {
1477 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_i] = obj->
1478 Type->data[cartesian_trajectory_planner_B.b_kstr_i];
1479 }
1480
1481 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1482 cartesian_trajectory_planner_B.b_kstr_i < 5;
1483 cartesian_trajectory_planner_B.b_kstr_i++) {
1484 cartesian_trajectory_planner_B.b_e[cartesian_trajectory_planner_B.b_kstr_i] =
1485 tmp[cartesian_trajectory_planner_B.b_kstr_i];
1486 }
1487
1488 cartesian_trajectory_planner_B.b_bool_k = false;
1489 if (switch_expression->size[1] == 5) {
1490 cartesian_trajectory_planner_B.b_kstr_i = 1;
1491 do {
1492 exitg1 = 0;
1493 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 5) {
1494 cartesian_trajectory_planner_B.loop_ub_h5 =
1495 cartesian_trajectory_planner_B.b_kstr_i - 1;
1496 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_h5]
1497 !=
1498 cartesian_trajectory_planner_B.b_e[cartesian_trajectory_planner_B.loop_ub_h5])
1499 {
1500 exitg1 = 1;
1501 } else {
1502 cartesian_trajectory_planner_B.b_kstr_i++;
1503 }
1504 } else {
1505 cartesian_trajectory_planner_B.b_bool_k = true;
1506 exitg1 = 1;
1507 }
1508 } while (exitg1 == 0);
1509 }
1510
1511 if (cartesian_trajectory_planner_B.b_bool_k) {
1512 cartesian_trajectory_planner_B.b_kstr_i = 0;
1513 } else {
1514 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1515 cartesian_trajectory_planner_B.b_kstr_i < 8;
1516 cartesian_trajectory_planner_B.b_kstr_i++) {
1517 cartesian_trajectory_planner_B.b_m[cartesian_trajectory_planner_B.b_kstr_i]
1518 = tmp_0[cartesian_trajectory_planner_B.b_kstr_i];
1519 }
1520
1521 cartesian_trajectory_planner_B.b_bool_k = false;
1522 if (switch_expression->size[1] == 8) {
1523 cartesian_trajectory_planner_B.b_kstr_i = 1;
1524 do {
1525 exitg1 = 0;
1526 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 8) {
1527 cartesian_trajectory_planner_B.loop_ub_h5 =
1528 cartesian_trajectory_planner_B.b_kstr_i - 1;
1529 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_h5]
1530 !=
1531 cartesian_trajectory_planner_B.b_m[cartesian_trajectory_planner_B.loop_ub_h5])
1532 {
1533 exitg1 = 1;
1534 } else {
1535 cartesian_trajectory_planner_B.b_kstr_i++;
1536 }
1537 } else {
1538 cartesian_trajectory_planner_B.b_bool_k = true;
1539 exitg1 = 1;
1540 }
1541 } while (exitg1 == 0);
1542 }
1543
1544 if (cartesian_trajectory_planner_B.b_bool_k) {
1545 cartesian_trajectory_planner_B.b_kstr_i = 1;
1546 } else {
1547 cartesian_trajectory_planner_B.b_kstr_i = -1;
1548 }
1549 }
1550
1551 cartesian_trajec_emxFree_char_T(&switch_expression);
1552 switch (cartesian_trajectory_planner_B.b_kstr_i) {
1553 case 0:
1554 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1555 cartesian_trajectory_planner_B.TJ[0] = 1.0;
1556 cartesian_trajectory_planner_B.TJ[5] = 1.0;
1557 cartesian_trajectory_planner_B.TJ[10] = 1.0;
1558 cartesian_trajectory_planner_B.TJ[15] = 1.0;
1559 break;
1560
1561 case 1:
1562 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_jg);
1563 cartesian_trajectory_planner_B.loop_ub_h5 = (*q_size != 0) - 1;
1564 cartesian_trajectory_planner_B.result_data[0] =
1565 cartesian_trajectory_planner_B.v_jg[0];
1566 cartesian_trajectory_planner_B.result_data[1] =
1567 cartesian_trajectory_planner_B.v_jg[1];
1568 cartesian_trajectory_planner_B.result_data[2] =
1569 cartesian_trajectory_planner_B.v_jg[2];
1570 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1571 cartesian_trajectory_planner_B.b_kstr_i <=
1572 cartesian_trajectory_planner_B.loop_ub_h5;
1573 cartesian_trajectory_planner_B.b_kstr_i++) {
1574 cartesian_trajectory_planner_B.result_data[3] = q_data[0];
1575 }
1576
1577 cartesian_trajectory_planner_B.cth = 1.0 / sqrt
1578 ((cartesian_trajectory_planner_B.result_data[0] *
1579 cartesian_trajectory_planner_B.result_data[0] +
1580 cartesian_trajectory_planner_B.result_data[1] *
1581 cartesian_trajectory_planner_B.result_data[1]) +
1582 cartesian_trajectory_planner_B.result_data[2] *
1583 cartesian_trajectory_planner_B.result_data[2]);
1584 cartesian_trajectory_planner_B.v_jg[0] =
1585 cartesian_trajectory_planner_B.result_data[0] *
1586 cartesian_trajectory_planner_B.cth;
1587 cartesian_trajectory_planner_B.v_jg[1] =
1588 cartesian_trajectory_planner_B.result_data[1] *
1589 cartesian_trajectory_planner_B.cth;
1590 cartesian_trajectory_planner_B.v_jg[2] =
1591 cartesian_trajectory_planner_B.result_data[2] *
1592 cartesian_trajectory_planner_B.cth;
1593 cartesian_trajectory_planner_B.cth = cos
1594 (cartesian_trajectory_planner_B.result_data[3]);
1595 cartesian_trajectory_planner_B.sth = sin
1596 (cartesian_trajectory_planner_B.result_data[3]);
1597 cartesian_trajectory_planner_B.tempR_tmp =
1598 cartesian_trajectory_planner_B.v_jg[1] *
1599 cartesian_trajectory_planner_B.v_jg[0] * (1.0 -
1600 cartesian_trajectory_planner_B.cth);
1601 cartesian_trajectory_planner_B.tempR_tmp_n =
1602 cartesian_trajectory_planner_B.v_jg[2] *
1603 cartesian_trajectory_planner_B.sth;
1604 cartesian_trajectory_planner_B.tempR_tmp_l =
1605 cartesian_trajectory_planner_B.v_jg[2] *
1606 cartesian_trajectory_planner_B.v_jg[0] * (1.0 -
1607 cartesian_trajectory_planner_B.cth);
1608 cartesian_trajectory_planner_B.tempR_tmp_c =
1609 cartesian_trajectory_planner_B.v_jg[1] *
1610 cartesian_trajectory_planner_B.sth;
1611 cartesian_trajectory_planner_B.tempR_tmp_nc =
1612 cartesian_trajectory_planner_B.v_jg[2] *
1613 cartesian_trajectory_planner_B.v_jg[1] * (1.0 -
1614 cartesian_trajectory_planner_B.cth);
1615 cartesian_trajectory_planner_B.sth *= cartesian_trajectory_planner_B.v_jg[0];
1616 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_jg[0] *
1617 cartesian_trajectory_planner_B.v_jg[0] * (1.0 -
1618 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
1619 cartesian_trajectory_planner_B.tempR_tmp -
1620 cartesian_trajectory_planner_B.tempR_tmp_n,
1621 cartesian_trajectory_planner_B.tempR_tmp_l +
1622 cartesian_trajectory_planner_B.tempR_tmp_c,
1623 cartesian_trajectory_planner_B.tempR_tmp +
1624 cartesian_trajectory_planner_B.tempR_tmp_n,
1625 cartesian_trajectory_planner_B.v_jg[1] *
1626 cartesian_trajectory_planner_B.v_jg[1] * (1.0 -
1627 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
1628 cartesian_trajectory_planner_B.tempR_tmp_nc -
1629 cartesian_trajectory_planner_B.sth,
1630 cartesian_trajectory_planner_B.tempR_tmp_l -
1631 cartesian_trajectory_planner_B.tempR_tmp_c,
1632 cartesian_trajectory_planner_B.tempR_tmp_nc +
1633 cartesian_trajectory_planner_B.sth, cartesian_trajectory_planner_B.v_jg[2]
1634 * cartesian_trajectory_planner_B.v_jg[2] * (1.0 -
1635 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
1636 cartesian_trajectory_planner_B.tempR_l);
1637 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1638 cartesian_trajectory_planner_B.b_kstr_i < 3;
1639 cartesian_trajectory_planner_B.b_kstr_i++) {
1640 cartesian_trajectory_planner_B.loop_ub_h5 =
1641 cartesian_trajectory_planner_B.b_kstr_i + 1;
1642 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.loop_ub_h5
1643 - 1] = cartesian_trajectory_planner_B.tempR_l
1644 [(cartesian_trajectory_planner_B.loop_ub_h5 - 1) * 3];
1645 cartesian_trajectory_planner_B.loop_ub_h5 =
1646 cartesian_trajectory_planner_B.b_kstr_i + 1;
1647 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.loop_ub_h5
1648 + 2] = cartesian_trajectory_planner_B.tempR_l
1649 [(cartesian_trajectory_planner_B.loop_ub_h5 - 1) * 3 + 1];
1650 cartesian_trajectory_planner_B.loop_ub_h5 =
1651 cartesian_trajectory_planner_B.b_kstr_i + 1;
1652 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.loop_ub_h5
1653 + 5] = cartesian_trajectory_planner_B.tempR_l
1654 [(cartesian_trajectory_planner_B.loop_ub_h5 - 1) * 3 + 2];
1655 }
1656
1657 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1658 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1659 cartesian_trajectory_planner_B.b_kstr_i < 3;
1660 cartesian_trajectory_planner_B.b_kstr_i++) {
1661 cartesian_trajectory_planner_B.loop_ub_h5 =
1662 cartesian_trajectory_planner_B.b_kstr_i << 2;
1663 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5]
1664 = cartesian_trajectory_planner_B.R_o[3 *
1665 cartesian_trajectory_planner_B.b_kstr_i];
1666 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5
1667 + 1] = cartesian_trajectory_planner_B.R_o[3 *
1668 cartesian_trajectory_planner_B.b_kstr_i + 1];
1669 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5
1670 + 2] = cartesian_trajectory_planner_B.R_o[3 *
1671 cartesian_trajectory_planner_B.b_kstr_i + 2];
1672 }
1673
1674 cartesian_trajectory_planner_B.TJ[15] = 1.0;
1675 break;
1676
1677 default:
1678 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_jg);
1679 memset(&cartesian_trajectory_planner_B.tempR_l[0], 0, 9U * sizeof(real_T));
1680 cartesian_trajectory_planner_B.tempR_l[0] = 1.0;
1681 cartesian_trajectory_planner_B.tempR_l[4] = 1.0;
1682 cartesian_trajectory_planner_B.tempR_l[8] = 1.0;
1683 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1684 cartesian_trajectory_planner_B.b_kstr_i < 3;
1685 cartesian_trajectory_planner_B.b_kstr_i++) {
1686 cartesian_trajectory_planner_B.loop_ub_h5 =
1687 cartesian_trajectory_planner_B.b_kstr_i << 2;
1688 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5]
1689 = cartesian_trajectory_planner_B.tempR_l[3 *
1690 cartesian_trajectory_planner_B.b_kstr_i];
1691 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5
1692 + 1] = cartesian_trajectory_planner_B.tempR_l[3 *
1693 cartesian_trajectory_planner_B.b_kstr_i + 1];
1694 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_h5
1695 + 2] = cartesian_trajectory_planner_B.tempR_l[3 *
1696 cartesian_trajectory_planner_B.b_kstr_i + 2];
1697 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.b_kstr_i
1698 + 12] =
1699 cartesian_trajectory_planner_B.v_jg[cartesian_trajectory_planner_B.b_kstr_i]
1700 * q_data[0];
1701 }
1702
1703 cartesian_trajectory_planner_B.TJ[3] = 0.0;
1704 cartesian_trajectory_planner_B.TJ[7] = 0.0;
1705 cartesian_trajectory_planner_B.TJ[11] = 0.0;
1706 cartesian_trajectory_planner_B.TJ[15] = 1.0;
1707 break;
1708 }
1709
1710 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1711 cartesian_trajectory_planner_B.b_kstr_i < 16;
1712 cartesian_trajectory_planner_B.b_kstr_i++) {
1713 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_i] =
1714 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_i];
1715 }
1716
1717 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1718 cartesian_trajectory_planner_B.b_kstr_i < 16;
1719 cartesian_trajectory_planner_B.b_kstr_i++) {
1720 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.b_kstr_i] =
1721 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_i];
1722 }
1723
1724 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
1725 cartesian_trajectory_planner_B.b_kstr_i < 4;
1726 cartesian_trajectory_planner_B.b_kstr_i++) {
1727 for (cartesian_trajectory_planner_B.loop_ub_h5 = 0;
1728 cartesian_trajectory_planner_B.loop_ub_h5 < 4;
1729 cartesian_trajectory_planner_B.loop_ub_h5++) {
1730 cartesian_trajectory_planner_B.a_tmp_tmp =
1731 cartesian_trajectory_planner_B.loop_ub_h5 << 2;
1732 cartesian_trajectory_planner_B.a_tmp =
1733 cartesian_trajectory_planner_B.b_kstr_i +
1734 cartesian_trajectory_planner_B.a_tmp_tmp;
1735 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp] =
1736 0.0;
1737 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp] +=
1738 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp]
1739 * cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_i];
1740 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp] +=
1741 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
1742 + 1] *
1743 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_i
1744 + 4];
1745 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp] +=
1746 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
1747 + 2] *
1748 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_i
1749 + 8];
1750 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp] +=
1751 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
1752 + 3] *
1753 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_i
1754 + 12];
1755 }
1756
1757 for (cartesian_trajectory_planner_B.loop_ub_h5 = 0;
1758 cartesian_trajectory_planner_B.loop_ub_h5 < 4;
1759 cartesian_trajectory_planner_B.loop_ub_h5++) {
1760 cartesian_trajectory_planner_B.a_tmp_tmp =
1761 cartesian_trajectory_planner_B.loop_ub_h5 << 2;
1762 cartesian_trajectory_planner_B.a_tmp =
1763 cartesian_trajectory_planner_B.b_kstr_i +
1764 cartesian_trajectory_planner_B.a_tmp_tmp;
1765 T[cartesian_trajectory_planner_B.a_tmp] = 0.0;
1766 T[cartesian_trajectory_planner_B.a_tmp] +=
1767 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp]
1768 * cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_i];
1769 T[cartesian_trajectory_planner_B.a_tmp] +=
1770 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
1771 + 1] *
1772 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_i
1773 + 4];
1774 T[cartesian_trajectory_planner_B.a_tmp] +=
1775 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
1776 + 2] *
1777 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_i
1778 + 8];
1779 T[cartesian_trajectory_planner_B.a_tmp] +=
1780 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
1781 + 3] *
1782 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_i
1783 + 12];
1784 }
1785 }
1786}
1787
1788static void rigidBodyJoint_transformBodyToP(const
1789 c_rigidBodyJoint_cartesian_as_T *obj, real_T T[16])
1790{
1791 emxArray_char_T_cartesian_tra_T *switch_expression;
1792 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1793
1794 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1795
1796 int32_T exitg1;
1797 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1798 cartesian_trajectory_planner_B.b_kstr_j = switch_expression->size[0] *
1799 switch_expression->size[1];
1800 switch_expression->size[0] = 1;
1801 switch_expression->size[1] = obj->Type->size[1];
1802 cartes_emxEnsureCapacity_char_T(switch_expression,
1803 cartesian_trajectory_planner_B.b_kstr_j);
1804 cartesian_trajectory_planner_B.loop_ub_ga = obj->Type->size[0] * obj->
1805 Type->size[1] - 1;
1806 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1807 cartesian_trajectory_planner_B.b_kstr_j <=
1808 cartesian_trajectory_planner_B.loop_ub_ga;
1809 cartesian_trajectory_planner_B.b_kstr_j++) {
1810 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_j] = obj->
1811 Type->data[cartesian_trajectory_planner_B.b_kstr_j];
1812 }
1813
1814 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1815 cartesian_trajectory_planner_B.b_kstr_j < 5;
1816 cartesian_trajectory_planner_B.b_kstr_j++) {
1817 cartesian_trajectory_planner_B.b_hn[cartesian_trajectory_planner_B.b_kstr_j]
1818 = tmp[cartesian_trajectory_planner_B.b_kstr_j];
1819 }
1820
1821 cartesian_trajectory_planner_B.b_bool_l = false;
1822 if (switch_expression->size[1] == 5) {
1823 cartesian_trajectory_planner_B.b_kstr_j = 1;
1824 do {
1825 exitg1 = 0;
1826 if (cartesian_trajectory_planner_B.b_kstr_j - 1 < 5) {
1827 cartesian_trajectory_planner_B.loop_ub_ga =
1828 cartesian_trajectory_planner_B.b_kstr_j - 1;
1829 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_ga]
1830 !=
1831 cartesian_trajectory_planner_B.b_hn[cartesian_trajectory_planner_B.loop_ub_ga])
1832 {
1833 exitg1 = 1;
1834 } else {
1835 cartesian_trajectory_planner_B.b_kstr_j++;
1836 }
1837 } else {
1838 cartesian_trajectory_planner_B.b_bool_l = true;
1839 exitg1 = 1;
1840 }
1841 } while (exitg1 == 0);
1842 }
1843
1844 if (cartesian_trajectory_planner_B.b_bool_l) {
1845 cartesian_trajectory_planner_B.b_kstr_j = 0;
1846 } else {
1847 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1848 cartesian_trajectory_planner_B.b_kstr_j < 8;
1849 cartesian_trajectory_planner_B.b_kstr_j++) {
1850 cartesian_trajectory_planner_B.b_kn[cartesian_trajectory_planner_B.b_kstr_j]
1851 = tmp_0[cartesian_trajectory_planner_B.b_kstr_j];
1852 }
1853
1854 cartesian_trajectory_planner_B.b_bool_l = false;
1855 if (switch_expression->size[1] == 8) {
1856 cartesian_trajectory_planner_B.b_kstr_j = 1;
1857 do {
1858 exitg1 = 0;
1859 if (cartesian_trajectory_planner_B.b_kstr_j - 1 < 8) {
1860 cartesian_trajectory_planner_B.loop_ub_ga =
1861 cartesian_trajectory_planner_B.b_kstr_j - 1;
1862 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_ga]
1863 !=
1864 cartesian_trajectory_planner_B.b_kn[cartesian_trajectory_planner_B.loop_ub_ga])
1865 {
1866 exitg1 = 1;
1867 } else {
1868 cartesian_trajectory_planner_B.b_kstr_j++;
1869 }
1870 } else {
1871 cartesian_trajectory_planner_B.b_bool_l = true;
1872 exitg1 = 1;
1873 }
1874 } while (exitg1 == 0);
1875 }
1876
1877 if (cartesian_trajectory_planner_B.b_bool_l) {
1878 cartesian_trajectory_planner_B.b_kstr_j = 1;
1879 } else {
1880 cartesian_trajectory_planner_B.b_kstr_j = -1;
1881 }
1882 }
1883
1884 cartesian_trajec_emxFree_char_T(&switch_expression);
1885 switch (cartesian_trajectory_planner_B.b_kstr_j) {
1886 case 0:
1887 memset(&cartesian_trajectory_planner_B.TJ_g[0], 0, sizeof(real_T) << 4U);
1888 cartesian_trajectory_planner_B.TJ_g[0] = 1.0;
1889 cartesian_trajectory_planner_B.TJ_g[5] = 1.0;
1890 cartesian_trajectory_planner_B.TJ_g[10] = 1.0;
1891 cartesian_trajectory_planner_B.TJ_g[15] = 1.0;
1892 break;
1893
1894 case 1:
1895 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_f);
1896 cartesian_trajectory_planner_B.axang_idx_0 =
1897 cartesian_trajectory_planner_B.v_f[0];
1898 cartesian_trajectory_planner_B.axang_idx_1 =
1899 cartesian_trajectory_planner_B.v_f[1];
1900 cartesian_trajectory_planner_B.axang_idx_2 =
1901 cartesian_trajectory_planner_B.v_f[2];
1902 cartesian_trajectory_planner_B.b_pi = 1.0 / sqrt
1903 ((cartesian_trajectory_planner_B.axang_idx_0 *
1904 cartesian_trajectory_planner_B.axang_idx_0 +
1905 cartesian_trajectory_planner_B.axang_idx_1 *
1906 cartesian_trajectory_planner_B.axang_idx_1) +
1907 cartesian_trajectory_planner_B.axang_idx_2 *
1908 cartesian_trajectory_planner_B.axang_idx_2);
1909 cartesian_trajectory_planner_B.v_f[0] =
1910 cartesian_trajectory_planner_B.axang_idx_0 *
1911 cartesian_trajectory_planner_B.b_pi;
1912 cartesian_trajectory_planner_B.v_f[1] =
1913 cartesian_trajectory_planner_B.axang_idx_1 *
1914 cartesian_trajectory_planner_B.b_pi;
1915 cartesian_trajectory_planner_B.v_f[2] =
1916 cartesian_trajectory_planner_B.axang_idx_2 *
1917 cartesian_trajectory_planner_B.b_pi;
1918 cartesian_trajectory_planner_B.axang_idx_0 =
1919 cartesian_trajectory_planner_B.v_f[1] *
1920 cartesian_trajectory_planner_B.v_f[0] * 0.0;
1921 cartesian_trajectory_planner_B.axang_idx_1 =
1922 cartesian_trajectory_planner_B.v_f[2] *
1923 cartesian_trajectory_planner_B.v_f[0] * 0.0;
1924 cartesian_trajectory_planner_B.axang_idx_2 =
1925 cartesian_trajectory_planner_B.v_f[2] *
1926 cartesian_trajectory_planner_B.v_f[1] * 0.0;
1927 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_f[0] *
1928 cartesian_trajectory_planner_B.v_f[0] * 0.0 + 1.0,
1929 cartesian_trajectory_planner_B.axang_idx_0 -
1930 cartesian_trajectory_planner_B.v_f[2] * 0.0,
1931 cartesian_trajectory_planner_B.axang_idx_1 +
1932 cartesian_trajectory_planner_B.v_f[1] * 0.0,
1933 cartesian_trajectory_planner_B.axang_idx_0 +
1934 cartesian_trajectory_planner_B.v_f[2] * 0.0,
1935 cartesian_trajectory_planner_B.v_f[1] *
1936 cartesian_trajectory_planner_B.v_f[1] * 0.0 + 1.0,
1937 cartesian_trajectory_planner_B.axang_idx_2 -
1938 cartesian_trajectory_planner_B.v_f[0] * 0.0,
1939 cartesian_trajectory_planner_B.axang_idx_1 -
1940 cartesian_trajectory_planner_B.v_f[1] * 0.0,
1941 cartesian_trajectory_planner_B.axang_idx_2 +
1942 cartesian_trajectory_planner_B.v_f[0] * 0.0,
1943 cartesian_trajectory_planner_B.v_f[2] *
1944 cartesian_trajectory_planner_B.v_f[2] * 0.0 + 1.0,
1945 cartesian_trajectory_planner_B.tempR_m);
1946 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1947 cartesian_trajectory_planner_B.b_kstr_j < 3;
1948 cartesian_trajectory_planner_B.b_kstr_j++) {
1949 cartesian_trajectory_planner_B.loop_ub_ga =
1950 cartesian_trajectory_planner_B.b_kstr_j + 1;
1951 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.loop_ub_ga
1952 - 1] = cartesian_trajectory_planner_B.tempR_m
1953 [(cartesian_trajectory_planner_B.loop_ub_ga - 1) * 3];
1954 cartesian_trajectory_planner_B.loop_ub_ga =
1955 cartesian_trajectory_planner_B.b_kstr_j + 1;
1956 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.loop_ub_ga
1957 + 2] = cartesian_trajectory_planner_B.tempR_m
1958 [(cartesian_trajectory_planner_B.loop_ub_ga - 1) * 3 + 1];
1959 cartesian_trajectory_planner_B.loop_ub_ga =
1960 cartesian_trajectory_planner_B.b_kstr_j + 1;
1961 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.loop_ub_ga
1962 + 5] = cartesian_trajectory_planner_B.tempR_m
1963 [(cartesian_trajectory_planner_B.loop_ub_ga - 1) * 3 + 2];
1964 }
1965
1966 memset(&cartesian_trajectory_planner_B.TJ_g[0], 0, sizeof(real_T) << 4U);
1967 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1968 cartesian_trajectory_planner_B.b_kstr_j < 3;
1969 cartesian_trajectory_planner_B.b_kstr_j++) {
1970 cartesian_trajectory_planner_B.loop_ub_ga =
1971 cartesian_trajectory_planner_B.b_kstr_j << 2;
1972 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga]
1973 = cartesian_trajectory_planner_B.R_m[3 *
1974 cartesian_trajectory_planner_B.b_kstr_j];
1975 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga
1976 + 1] = cartesian_trajectory_planner_B.R_m[3 *
1977 cartesian_trajectory_planner_B.b_kstr_j + 1];
1978 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga
1979 + 2] = cartesian_trajectory_planner_B.R_m[3 *
1980 cartesian_trajectory_planner_B.b_kstr_j + 2];
1981 }
1982
1983 cartesian_trajectory_planner_B.TJ_g[15] = 1.0;
1984 break;
1985
1986 default:
1987 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_f);
1988 memset(&cartesian_trajectory_planner_B.tempR_m[0], 0, 9U * sizeof(real_T));
1989 cartesian_trajectory_planner_B.tempR_m[0] = 1.0;
1990 cartesian_trajectory_planner_B.tempR_m[4] = 1.0;
1991 cartesian_trajectory_planner_B.tempR_m[8] = 1.0;
1992 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
1993 cartesian_trajectory_planner_B.b_kstr_j < 3;
1994 cartesian_trajectory_planner_B.b_kstr_j++) {
1995 cartesian_trajectory_planner_B.loop_ub_ga =
1996 cartesian_trajectory_planner_B.b_kstr_j << 2;
1997 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga]
1998 = cartesian_trajectory_planner_B.tempR_m[3 *
1999 cartesian_trajectory_planner_B.b_kstr_j];
2000 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga
2001 + 1] = cartesian_trajectory_planner_B.tempR_m[3 *
2002 cartesian_trajectory_planner_B.b_kstr_j + 1];
2003 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.loop_ub_ga
2004 + 2] = cartesian_trajectory_planner_B.tempR_m[3 *
2005 cartesian_trajectory_planner_B.b_kstr_j + 2];
2006 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.b_kstr_j
2007 + 12] =
2008 cartesian_trajectory_planner_B.v_f[cartesian_trajectory_planner_B.b_kstr_j]
2009 * 0.0;
2010 }
2011
2012 cartesian_trajectory_planner_B.TJ_g[3] = 0.0;
2013 cartesian_trajectory_planner_B.TJ_g[7] = 0.0;
2014 cartesian_trajectory_planner_B.TJ_g[11] = 0.0;
2015 cartesian_trajectory_planner_B.TJ_g[15] = 1.0;
2016 break;
2017 }
2018
2019 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
2020 cartesian_trajectory_planner_B.b_kstr_j < 16;
2021 cartesian_trajectory_planner_B.b_kstr_j++) {
2022 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_j] =
2023 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_j];
2024 }
2025
2026 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
2027 cartesian_trajectory_planner_B.b_kstr_j < 16;
2028 cartesian_trajectory_planner_B.b_kstr_j++) {
2029 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.b_kstr_j] =
2030 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_j];
2031 }
2032
2033 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
2034 cartesian_trajectory_planner_B.b_kstr_j < 4;
2035 cartesian_trajectory_planner_B.b_kstr_j++) {
2036 for (cartesian_trajectory_planner_B.loop_ub_ga = 0;
2037 cartesian_trajectory_planner_B.loop_ub_ga < 4;
2038 cartesian_trajectory_planner_B.loop_ub_ga++) {
2039 cartesian_trajectory_planner_B.a_tmp_tmp_e =
2040 cartesian_trajectory_planner_B.loop_ub_ga << 2;
2041 cartesian_trajectory_planner_B.a_tmp_j =
2042 cartesian_trajectory_planner_B.b_kstr_j +
2043 cartesian_trajectory_planner_B.a_tmp_tmp_e;
2044 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.a_tmp_j]
2045 = 0.0;
2046 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.a_tmp_j]
2047 +=
2048 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.a_tmp_tmp_e]
2049 * cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_j];
2050 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.a_tmp_j]
2051 +=
2052 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.a_tmp_tmp_e
2053 + 1] *
2054 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_j
2055 + 4];
2056 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.a_tmp_j]
2057 +=
2058 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.a_tmp_tmp_e
2059 + 2] *
2060 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_j
2061 + 8];
2062 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.a_tmp_j]
2063 +=
2064 cartesian_trajectory_planner_B.TJ_g[cartesian_trajectory_planner_B.a_tmp_tmp_e
2065 + 3] *
2066 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_j
2067 + 12];
2068 }
2069
2070 for (cartesian_trajectory_planner_B.loop_ub_ga = 0;
2071 cartesian_trajectory_planner_B.loop_ub_ga < 4;
2072 cartesian_trajectory_planner_B.loop_ub_ga++) {
2073 cartesian_trajectory_planner_B.a_tmp_tmp_e =
2074 cartesian_trajectory_planner_B.loop_ub_ga << 2;
2075 cartesian_trajectory_planner_B.a_tmp_j =
2076 cartesian_trajectory_planner_B.b_kstr_j +
2077 cartesian_trajectory_planner_B.a_tmp_tmp_e;
2078 T[cartesian_trajectory_planner_B.a_tmp_j] = 0.0;
2079 T[cartesian_trajectory_planner_B.a_tmp_j] +=
2080 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.a_tmp_tmp_e]
2081 * cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.b_kstr_j];
2082 T[cartesian_trajectory_planner_B.a_tmp_j] +=
2083 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.a_tmp_tmp_e
2084 + 1] *
2085 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.b_kstr_j
2086 + 4];
2087 T[cartesian_trajectory_planner_B.a_tmp_j] +=
2088 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.a_tmp_tmp_e
2089 + 2] *
2090 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.b_kstr_j
2091 + 8];
2092 T[cartesian_trajectory_planner_B.a_tmp_j] +=
2093 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.a_tmp_tmp_e
2094 + 3] *
2095 cartesian_trajectory_planner_B.a_dy[cartesian_trajectory_planner_B.b_kstr_j
2096 + 12];
2097 }
2098 }
2099}
2100
2101static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
2102 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
2103 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac)
2104{
2105 w_robotics_manip_internal_Rig_T *body1;
2106 w_robotics_manip_internal_Rig_T *body2;
2107 emxArray_real_T_cartesian_tra_T *kinematicPathIndices;
2108 c_rigidBodyJoint_cartesian_as_T *joint;
2109 emxArray_char_T_cartesian_tra_T *body2Name;
2110 emxArray_real_T_cartesian_tra_T *ancestorIndices1;
2111 emxArray_real_T_cartesian_tra_T *ancestorIndices2;
2112 emxArray_real_T_cartesian_tra_T *y;
2113 emxArray_real_T_cartesian_tra_T *b;
2114 w_robotics_manip_internal_Rig_T *body;
2115 emxArray_char_T_cartesian_tra_T *bname;
2116 emxArray_real_T_cartesian_tra_T *ancestorIndices1_0;
2117 emxArray_real_T_cartesian_tra_T *ancestorIndices2_0;
2118 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
2119
2120 boolean_T exitg1;
2121 int32_T exitg2;
2122 cartesian_trajec_emxInit_char_T(&body2Name, 2);
2123 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];
2124 body2Name->size[0] = 1;
2125 body2Name->size[1] = obj->Base.NameInternal->size[1];
2126 cartes_emxEnsureCapacity_char_T(body2Name,
2127 cartesian_trajectory_planner_B.b_kstr);
2128 cartesian_trajectory_planner_B.loop_ub_k = obj->Base.NameInternal->size[0] *
2129 obj->Base.NameInternal->size[1] - 1;
2130 for (cartesian_trajectory_planner_B.b_kstr = 0;
2131 cartesian_trajectory_planner_B.b_kstr <=
2132 cartesian_trajectory_planner_B.loop_ub_k;
2133 cartesian_trajectory_planner_B.b_kstr++) {
2134 body2Name->data[cartesian_trajectory_planner_B.b_kstr] =
2135 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2136 }
2137
2138 cartesian_trajec_emxInit_char_T(&bname, 2);
2139 cartesian_trajectory_planner_B.bid1 = -1.0;
2140 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2141 bname->size[0] = 1;
2142 bname->size[1] = obj->Base.NameInternal->size[1];
2143 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr);
2144 cartesian_trajectory_planner_B.loop_ub_k = obj->Base.NameInternal->size[0] *
2145 obj->Base.NameInternal->size[1] - 1;
2146 for (cartesian_trajectory_planner_B.b_kstr = 0;
2147 cartesian_trajectory_planner_B.b_kstr <=
2148 cartesian_trajectory_planner_B.loop_ub_k;
2149 cartesian_trajectory_planner_B.b_kstr++) {
2150 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
2151 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2152 }
2153
2154 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
2155 cartesian_trajectory_planner_B.bid1 = 0.0;
2156 } else {
2157 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
2158 cartesian_trajectory_planner_B.b_i_d = 0;
2159 exitg1 = false;
2160 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_d <=
2161 static_cast<int32_T>
2162 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
2163 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_d];
2164 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2165 bname->size[0] = 1;
2166 bname->size[1] = body1->NameInternal->size[1];
2167 cartes_emxEnsureCapacity_char_T(bname,
2168 cartesian_trajectory_planner_B.b_kstr);
2169 cartesian_trajectory_planner_B.loop_ub_k = body1->NameInternal->size[0] *
2170 body1->NameInternal->size[1] - 1;
2171 for (cartesian_trajectory_planner_B.b_kstr = 0;
2172 cartesian_trajectory_planner_B.b_kstr <=
2173 cartesian_trajectory_planner_B.loop_ub_k;
2174 cartesian_trajectory_planner_B.b_kstr++) {
2175 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
2176 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2177 }
2178
2179 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
2180 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
2181 (cartesian_trajectory_planner_B.b_i_d) + 1.0;
2182 exitg1 = true;
2183 } else {
2184 cartesian_trajectory_planner_B.b_i_d++;
2185 }
2186 }
2187 }
2188
2189 cartesian_trajectory_planner_B.bid2 = -1.0;
2190 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2191 bname->size[0] = 1;
2192 bname->size[1] = obj->Base.NameInternal->size[1];
2193 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr);
2194 cartesian_trajectory_planner_B.loop_ub_k = obj->Base.NameInternal->size[0] *
2195 obj->Base.NameInternal->size[1] - 1;
2196 for (cartesian_trajectory_planner_B.b_kstr = 0;
2197 cartesian_trajectory_planner_B.b_kstr <=
2198 cartesian_trajectory_planner_B.loop_ub_k;
2199 cartesian_trajectory_planner_B.b_kstr++) {
2200 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
2201 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2202 }
2203
2204 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
2205 cartesian_trajectory_planner_B.bid2 = 0.0;
2206 } else {
2207 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
2208 cartesian_trajectory_planner_B.b_i_d = 0;
2209 exitg1 = false;
2210 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_d <=
2211 static_cast<int32_T>
2212 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
2213 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_d];
2214 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2215 bname->size[0] = 1;
2216 bname->size[1] = body1->NameInternal->size[1];
2217 cartes_emxEnsureCapacity_char_T(bname,
2218 cartesian_trajectory_planner_B.b_kstr);
2219 cartesian_trajectory_planner_B.loop_ub_k = body1->NameInternal->size[0] *
2220 body1->NameInternal->size[1] - 1;
2221 for (cartesian_trajectory_planner_B.b_kstr = 0;
2222 cartesian_trajectory_planner_B.b_kstr <=
2223 cartesian_trajectory_planner_B.loop_ub_k;
2224 cartesian_trajectory_planner_B.b_kstr++) {
2225 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
2226 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2227 }
2228
2229 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
2230 cartesian_trajectory_planner_B.bid2 = static_cast<real_T>
2231 (cartesian_trajectory_planner_B.b_i_d) + 1.0;
2232 exitg1 = true;
2233 } else {
2234 cartesian_trajectory_planner_B.b_i_d++;
2235 }
2236 }
2237 }
2238
2239 cartesian_trajec_emxFree_char_T(&bname);
2240 if (cartesian_trajectory_planner_B.bid1 == 0.0) {
2241 body1 = &obj->Base;
2242 } else {
2243 body1 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid1)
2244 - 1];
2245 }
2246
2247 if (cartesian_trajectory_planner_B.bid2 == 0.0) {
2248 body2 = &obj->Base;
2249 } else {
2250 body2 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid2)
2251 - 1];
2252 }
2253
2254 cartesian_trajec_emxInit_real_T(&ancestorIndices1, 2);
2255 body = body1;
2256 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
2257 ancestorIndices1->size[1];
2258 ancestorIndices1->size[0] = 1;
2259 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2260 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2261 cartesian_trajectory_planner_B.b_kstr);
2262 cartesian_trajectory_planner_B.loop_ub_k = static_cast<int32_T>(obj->NumBodies
2263 + 1.0) - 1;
2264 for (cartesian_trajectory_planner_B.b_kstr = 0;
2265 cartesian_trajectory_planner_B.b_kstr <=
2266 cartesian_trajectory_planner_B.loop_ub_k;
2267 cartesian_trajectory_planner_B.b_kstr++) {
2268 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2269 }
2270
2271 cartesian_trajectory_planner_B.bid1 = 2.0;
2272 ancestorIndices1->data[0] = body1->Index;
2273 while (body->ParentIndex > 0.0) {
2274 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2275 ancestorIndices1->data[static_cast<int32_T>
2276 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
2277 cartesian_trajectory_planner_B.bid1++;
2278 }
2279
2280 if (body->Index > 0.0) {
2281 ancestorIndices1->data[static_cast<int32_T>
2282 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
2283 cartesian_trajectory_planner_B.bid1++;
2284 }
2285
2286 cartesian_trajec_emxInit_real_T(&ancestorIndices1_0, 2);
2287 cartesian_trajectory_planner_B.loop_ub_k = static_cast<int32_T>
2288 (cartesian_trajectory_planner_B.bid1 - 1.0);
2289 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1_0->size[0] *
2290 ancestorIndices1_0->size[1];
2291 ancestorIndices1_0->size[0] = 1;
2292 ancestorIndices1_0->size[1] = cartesian_trajectory_planner_B.loop_ub_k;
2293 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,
2294 cartesian_trajectory_planner_B.b_kstr);
2295 for (cartesian_trajectory_planner_B.b_kstr = 0;
2296 cartesian_trajectory_planner_B.b_kstr <
2297 cartesian_trajectory_planner_B.loop_ub_k;
2298 cartesian_trajectory_planner_B.b_kstr++) {
2299 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr] =
2300 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
2301 }
2302
2303 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
2304 ancestorIndices1->size[1];
2305 ancestorIndices1->size[0] = 1;
2306 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
2307 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2308 cartesian_trajectory_planner_B.b_kstr);
2309 cartesian_trajectory_planner_B.loop_ub_k = ancestorIndices1_0->size[0] *
2310 ancestorIndices1_0->size[1];
2311 for (cartesian_trajectory_planner_B.b_kstr = 0;
2312 cartesian_trajectory_planner_B.b_kstr <
2313 cartesian_trajectory_planner_B.loop_ub_k;
2314 cartesian_trajectory_planner_B.b_kstr++) {
2315 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] =
2316 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr];
2317 }
2318
2319 cartesian_trajec_emxFree_real_T(&ancestorIndices1_0);
2320 cartesian_trajec_emxInit_real_T(&ancestorIndices2, 2);
2321 body = body2;
2322 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
2323 ancestorIndices2->size[1];
2324 ancestorIndices2->size[0] = 1;
2325 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2326 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2327 cartesian_trajectory_planner_B.b_kstr);
2328 cartesian_trajectory_planner_B.loop_ub_k = static_cast<int32_T>(obj->NumBodies
2329 + 1.0) - 1;
2330 for (cartesian_trajectory_planner_B.b_kstr = 0;
2331 cartesian_trajectory_planner_B.b_kstr <=
2332 cartesian_trajectory_planner_B.loop_ub_k;
2333 cartesian_trajectory_planner_B.b_kstr++) {
2334 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2335 }
2336
2337 cartesian_trajectory_planner_B.bid1 = 2.0;
2338 ancestorIndices2->data[0] = body2->Index;
2339 while (body->ParentIndex > 0.0) {
2340 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2341 ancestorIndices2->data[static_cast<int32_T>
2342 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
2343 cartesian_trajectory_planner_B.bid1++;
2344 }
2345
2346 if (body->Index > 0.0) {
2347 ancestorIndices2->data[static_cast<int32_T>
2348 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
2349 cartesian_trajectory_planner_B.bid1++;
2350 }
2351
2352 cartesian_trajec_emxInit_real_T(&ancestorIndices2_0, 2);
2353 cartesian_trajectory_planner_B.loop_ub_k = static_cast<int32_T>
2354 (cartesian_trajectory_planner_B.bid1 - 1.0);
2355 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2_0->size[0] *
2356 ancestorIndices2_0->size[1];
2357 ancestorIndices2_0->size[0] = 1;
2358 ancestorIndices2_0->size[1] = cartesian_trajectory_planner_B.loop_ub_k;
2359 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,
2360 cartesian_trajectory_planner_B.b_kstr);
2361 for (cartesian_trajectory_planner_B.b_kstr = 0;
2362 cartesian_trajectory_planner_B.b_kstr <
2363 cartesian_trajectory_planner_B.loop_ub_k;
2364 cartesian_trajectory_planner_B.b_kstr++) {
2365 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr] =
2366 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr];
2367 }
2368
2369 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
2370 ancestorIndices2->size[1];
2371 ancestorIndices2->size[0] = 1;
2372 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
2373 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2374 cartesian_trajectory_planner_B.b_kstr);
2375 cartesian_trajectory_planner_B.loop_ub_k = ancestorIndices2_0->size[0] *
2376 ancestorIndices2_0->size[1];
2377 for (cartesian_trajectory_planner_B.b_kstr = 0;
2378 cartesian_trajectory_planner_B.b_kstr <
2379 cartesian_trajectory_planner_B.loop_ub_k;
2380 cartesian_trajectory_planner_B.b_kstr++) {
2381 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] =
2382 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr];
2383 }
2384
2385 cartesian_trajec_emxFree_real_T(&ancestorIndices2_0);
2386 cartesian_trajectory_planner_B.bid1 = ancestorIndices1->size[1];
2387 cartesian_trajectory_planner_B.qidx_idx_1 = ancestorIndices2->size[1];
2388 if (cartesian_trajectory_planner_B.bid1 <
2389 cartesian_trajectory_planner_B.qidx_idx_1) {
2390 cartesian_trajectory_planner_B.qidx_idx_1 =
2391 cartesian_trajectory_planner_B.bid1;
2392 }
2393
2394 cartesian_trajectory_planner_B.bid1 = static_cast<int32_T>
2395 (cartesian_trajectory_planner_B.qidx_idx_1);
2396 cartesian_trajectory_planner_B.b_i_d = 0;
2397 exitg1 = false;
2398 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_d <=
2399 static_cast<int32_T>
2400 (cartesian_trajectory_planner_B.qidx_idx_1) - 2)) {
2401 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
2402 (ancestorIndices1->size[1]) - (static_cast<real_T>
2403 (cartesian_trajectory_planner_B.b_i_d) + 1.0)) - 1] !=
2404 ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
2405 (ancestorIndices2->size[1]) - (static_cast<real_T>
2406 (cartesian_trajectory_planner_B.b_i_d) + 1.0)) - 1]) {
2407 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
2408 (cartesian_trajectory_planner_B.b_i_d) + 1.0;
2409 exitg1 = true;
2410 } else {
2411 cartesian_trajectory_planner_B.b_i_d++;
2412 }
2413 }
2414
2415 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
2416 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1;
2417 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
2418 cartesian_trajectory_planner_B.b_i_d = -1;
2419 } else {
2420 cartesian_trajectory_planner_B.b_i_d = static_cast<int32_T>
2421 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
2422 }
2423
2424 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
2425 (ancestorIndices2->size[1]) - cartesian_trajectory_planner_B.bid1;
2426 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
2427 cartesian_trajectory_planner_B.j_f = 0;
2428 cartesian_trajectory_planner_B.jointSign = 1;
2429 cartesian_trajectory_planner_B.g = -1;
2430 } else {
2431 cartesian_trajectory_planner_B.j_f = static_cast<int32_T>
2432 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
2433 cartesian_trajectory_planner_B.jointSign = -1;
2434 cartesian_trajectory_planner_B.g = 0;
2435 }
2436
2437 cartesian_trajec_emxInit_real_T(&kinematicPathIndices, 2);
2438 cartesian_trajectory_planner_B.b_kstr = kinematicPathIndices->size[0] *
2439 kinematicPathIndices->size[1];
2440 kinematicPathIndices->size[0] = 1;
2441 cartesian_trajectory_planner_B.loop_ub_k = div_s32_floor
2442 (cartesian_trajectory_planner_B.g - cartesian_trajectory_planner_B.j_f,
2443 cartesian_trajectory_planner_B.jointSign);
2444 kinematicPathIndices->size[1] = (cartesian_trajectory_planner_B.loop_ub_k +
2445 cartesian_trajectory_planner_B.b_i_d) + 3;
2446 cartes_emxEnsureCapacity_real_T(kinematicPathIndices,
2447 cartesian_trajectory_planner_B.b_kstr);
2448 for (cartesian_trajectory_planner_B.b_kstr = 0;
2449 cartesian_trajectory_planner_B.b_kstr <=
2450 cartesian_trajectory_planner_B.b_i_d;
2451 cartesian_trajectory_planner_B.b_kstr++) {
2452 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] =
2453 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
2454 }
2455
2456 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_d + 1] =
2457 ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>
2458 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1) + 1.0) -
2459 1];
2460 cartesian_trajec_emxFree_real_T(&ancestorIndices1);
2461 for (cartesian_trajectory_planner_B.b_kstr = 0;
2462 cartesian_trajectory_planner_B.b_kstr <=
2463 cartesian_trajectory_planner_B.loop_ub_k;
2464 cartesian_trajectory_planner_B.b_kstr++) {
2465 kinematicPathIndices->data[(cartesian_trajectory_planner_B.b_kstr +
2466 cartesian_trajectory_planner_B.b_i_d) + 2] = ancestorIndices2->
2467 data[cartesian_trajectory_planner_B.jointSign *
2468 cartesian_trajectory_planner_B.b_kstr + cartesian_trajectory_planner_B.j_f];
2469 }
2470
2471 cartesian_trajec_emxFree_real_T(&ancestorIndices2);
2472 memset(&cartesian_trajectory_planner_B.T1[0], 0, sizeof(real_T) << 4U);
2473 cartesian_trajectory_planner_B.T1[0] = 1.0;
2474 cartesian_trajectory_planner_B.T1[5] = 1.0;
2475 cartesian_trajectory_planner_B.T1[10] = 1.0;
2476 cartesian_trajectory_planner_B.T1[15] = 1.0;
2477 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
2478 Jac->size[0] = 6;
2479 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
2480 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
2481 cartesian_trajectory_planner_B.loop_ub_k = 6 * static_cast<int32_T>
2482 (obj->PositionNumber) - 1;
2483 for (cartesian_trajectory_planner_B.b_kstr = 0;
2484 cartesian_trajectory_planner_B.b_kstr <=
2485 cartesian_trajectory_planner_B.loop_ub_k;
2486 cartesian_trajectory_planner_B.b_kstr++) {
2487 Jac->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2488 }
2489
2490 cartesian_trajectory_planner_B.j_f = kinematicPathIndices->size[1] - 2;
2491 cartesian_trajec_emxInit_real_T(&y, 2);
2492 cartesian_trajec_emxInit_real_T(&b, 2);
2493 if (0 <= cartesian_trajectory_planner_B.j_f) {
2494 for (cartesian_trajectory_planner_B.b_kstr = 0;
2495 cartesian_trajectory_planner_B.b_kstr < 5;
2496 cartesian_trajectory_planner_B.b_kstr++) {
2497 cartesian_trajectory_planner_B.b_h[cartesian_trajectory_planner_B.b_kstr] =
2498 tmp[cartesian_trajectory_planner_B.b_kstr];
2499 }
2500 }
2501
2502 for (cartesian_trajectory_planner_B.b_i_d = 0;
2503 cartesian_trajectory_planner_B.b_i_d <=
2504 cartesian_trajectory_planner_B.j_f; cartesian_trajectory_planner_B.b_i_d
2505 ++) {
2506 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_d] != 0.0)
2507 {
2508 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2509 data[cartesian_trajectory_planner_B.b_i_d]) - 1];
2510 } else {
2511 body1 = &obj->Base;
2512 }
2513
2514 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
2515 ((static_cast<real_T>(cartesian_trajectory_planner_B.b_i_d) + 1.0) + 1.0)
2516 - 1;
2517 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] != 0.0)
2518 {
2519 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2520 data[cartesian_trajectory_planner_B.b_kstr]) - 1];
2521 } else {
2522 body2 = &obj->Base;
2523 }
2524
2525 cartesian_trajectory_planner_B.nextBodyIsParent = (body2->Index ==
2526 body1->ParentIndex);
2527 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
2528 body2 = body1;
2529 cartesian_trajectory_planner_B.jointSign = 1;
2530 } else {
2531 cartesian_trajectory_planner_B.jointSign = -1;
2532 }
2533
2534 joint = body2->JointInternal;
2535 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->
2536 size[1];
2537 body2Name->size[0] = 1;
2538 body2Name->size[1] = joint->Type->size[1];
2539 cartes_emxEnsureCapacity_char_T(body2Name,
2540 cartesian_trajectory_planner_B.b_kstr);
2541 cartesian_trajectory_planner_B.loop_ub_k = joint->Type->size[0] *
2542 joint->Type->size[1] - 1;
2543 for (cartesian_trajectory_planner_B.b_kstr = 0;
2544 cartesian_trajectory_planner_B.b_kstr <=
2545 cartesian_trajectory_planner_B.loop_ub_k;
2546 cartesian_trajectory_planner_B.b_kstr++) {
2547 body2Name->data[cartesian_trajectory_planner_B.b_kstr] = joint->Type->
2548 data[cartesian_trajectory_planner_B.b_kstr];
2549 }
2550
2551 cartesian_trajectory_planner_B.b_bool = false;
2552 if (body2Name->size[1] == 5) {
2553 cartesian_trajectory_planner_B.b_kstr = 1;
2554 do {
2555 exitg2 = 0;
2556 if (cartesian_trajectory_planner_B.b_kstr - 1 < 5) {
2557 cartesian_trajectory_planner_B.g =
2558 cartesian_trajectory_planner_B.b_kstr - 1;
2559 if (body2Name->data[cartesian_trajectory_planner_B.g] !=
2560 cartesian_trajectory_planner_B.b_h[cartesian_trajectory_planner_B.g])
2561 {
2562 exitg2 = 1;
2563 } else {
2564 cartesian_trajectory_planner_B.b_kstr++;
2565 }
2566 } else {
2567 cartesian_trajectory_planner_B.b_bool = true;
2568 exitg2 = 1;
2569 }
2570 } while (exitg2 == 0);
2571 }
2572
2573 if (cartesian_trajectory_planner_B.b_bool) {
2574 rigidBodyJoint_transformBodyToP(joint, cartesian_trajectory_planner_B.Tc2p);
2575 } else {
2576 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2577 cartesian_trajectory_planner_B.bid1 = obj->
2578 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
2579 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
2580 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
2581 if (cartesian_trajectory_planner_B.bid1 >
2582 cartesian_trajectory_planner_B.qidx_idx_1) {
2583 cartesian_trajectory_planner_B.g = 0;
2584 cartesian_trajectory_planner_B.b_kstr = -1;
2585 } else {
2586 cartesian_trajectory_planner_B.g = static_cast<int32_T>
2587 (cartesian_trajectory_planner_B.bid1) - 1;
2588 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
2589 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
2590 }
2591
2592 cartesian_trajectory_planner_B.loop_ub_k =
2593 cartesian_trajectory_planner_B.b_kstr - cartesian_trajectory_planner_B.g;
2594 cartesian_trajectory_planner_B.qv_size =
2595 cartesian_trajectory_planner_B.loop_ub_k + 1;
2596 for (cartesian_trajectory_planner_B.b_kstr = 0;
2597 cartesian_trajectory_planner_B.b_kstr <=
2598 cartesian_trajectory_planner_B.loop_ub_k;
2599 cartesian_trajectory_planner_B.b_kstr++) {
2600 cartesian_trajectory_planner_B.qv_data[cartesian_trajectory_planner_B.b_kstr]
2601 = qv[cartesian_trajectory_planner_B.g +
2602 cartesian_trajectory_planner_B.b_kstr];
2603 }
2604
2605 rigidBodyJoint_transformBodyT_a(joint,
2606 cartesian_trajectory_planner_B.qv_data,
2607 &cartesian_trajectory_planner_B.qv_size,
2608 cartesian_trajectory_planner_B.Tc2p);
2609 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2610 cartesian_trajectory_planner_B.bid1 = obj->
2611 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
2612 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
2613 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
2614 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
2615 for (cartesian_trajectory_planner_B.b_kstr = 0;
2616 cartesian_trajectory_planner_B.b_kstr < 16;
2617 cartesian_trajectory_planner_B.b_kstr++) {
2618 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr]
2619 = joint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr];
2620 }
2621 } else {
2622 for (cartesian_trajectory_planner_B.b_kstr = 0;
2623 cartesian_trajectory_planner_B.b_kstr < 16;
2624 cartesian_trajectory_planner_B.b_kstr++) {
2625 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr]
2626 = joint->
2627 JointToParentTransform[cartesian_trajectory_planner_B.b_kstr];
2628 }
2629
2630 for (cartesian_trajectory_planner_B.b_kstr = 0;
2631 cartesian_trajectory_planner_B.b_kstr < 3;
2632 cartesian_trajectory_planner_B.b_kstr++) {
2633 cartesian_trajectory_planner_B.R_g[3 *
2634 cartesian_trajectory_planner_B.b_kstr] =
2635 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
2636 cartesian_trajectory_planner_B.R_g[3 *
2637 cartesian_trajectory_planner_B.b_kstr + 1] =
2638 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
2639 + 4];
2640 cartesian_trajectory_planner_B.R_g[3 *
2641 cartesian_trajectory_planner_B.b_kstr + 2] =
2642 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
2643 + 8];
2644 }
2645
2646 for (cartesian_trajectory_planner_B.b_kstr = 0;
2647 cartesian_trajectory_planner_B.b_kstr < 9;
2648 cartesian_trajectory_planner_B.b_kstr++) {
2649 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
2650 =
2651 -cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr];
2652 }
2653
2654 for (cartesian_trajectory_planner_B.b_kstr = 0;
2655 cartesian_trajectory_planner_B.b_kstr < 3;
2656 cartesian_trajectory_planner_B.b_kstr++) {
2657 cartesian_trajectory_planner_B.g =
2658 cartesian_trajectory_planner_B.b_kstr << 2;
2659 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
2660 cartesian_trajectory_planner_B.R_g[3 *
2661 cartesian_trajectory_planner_B.b_kstr];
2662 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1]
2663 = cartesian_trajectory_planner_B.R_g[3 *
2664 cartesian_trajectory_planner_B.b_kstr + 1];
2665 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2]
2666 = cartesian_trajectory_planner_B.R_g[3 *
2667 cartesian_trajectory_planner_B.b_kstr + 2];
2668 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
2669 + 12] =
2670 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
2671 + 6] * cartesian_trajectory_planner_B.T1j[14] +
2672 (cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
2673 + 3] * cartesian_trajectory_planner_B.T1j[13] +
2674 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
2675 * cartesian_trajectory_planner_B.T1j[12]);
2676 }
2677
2678 cartesian_trajectory_planner_B.Tj[3] = 0.0;
2679 cartesian_trajectory_planner_B.Tj[7] = 0.0;
2680 cartesian_trajectory_planner_B.Tj[11] = 0.0;
2681 cartesian_trajectory_planner_B.Tj[15] = 1.0;
2682 }
2683
2684 for (cartesian_trajectory_planner_B.b_kstr = 0;
2685 cartesian_trajectory_planner_B.b_kstr < 4;
2686 cartesian_trajectory_planner_B.b_kstr++) {
2687 for (cartesian_trajectory_planner_B.g = 0;
2688 cartesian_trajectory_planner_B.g < 4;
2689 cartesian_trajectory_planner_B.g++) {
2690 cartesian_trajectory_planner_B.loop_ub_k =
2691 cartesian_trajectory_planner_B.g << 2;
2692 cartesian_trajectory_planner_B.n_l =
2693 cartesian_trajectory_planner_B.b_kstr +
2694 cartesian_trajectory_planner_B.loop_ub_k;
2695 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_l]
2696 = 0.0;
2697 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_l]
2698 +=
2699 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k]
2700 * cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr];
2701 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_l]
2702 +=
2703 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2704 + 1] *
2705 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
2706 + 4];
2707 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_l]
2708 +=
2709 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2710 + 2] *
2711 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
2712 + 8];
2713 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_l]
2714 +=
2715 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2716 + 3] *
2717 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
2718 + 12];
2719 }
2720 }
2721
2722 for (cartesian_trajectory_planner_B.b_kstr = 0;
2723 cartesian_trajectory_planner_B.b_kstr < 3;
2724 cartesian_trajectory_planner_B.b_kstr++) {
2725 cartesian_trajectory_planner_B.R_g[3 *
2726 cartesian_trajectory_planner_B.b_kstr] =
2727 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
2728 cartesian_trajectory_planner_B.R_g[3 *
2729 cartesian_trajectory_planner_B.b_kstr + 1] =
2730 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
2731 + 4];
2732 cartesian_trajectory_planner_B.R_g[3 *
2733 cartesian_trajectory_planner_B.b_kstr + 2] =
2734 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
2735 + 8];
2736 }
2737
2738 for (cartesian_trajectory_planner_B.b_kstr = 0;
2739 cartesian_trajectory_planner_B.b_kstr < 9;
2740 cartesian_trajectory_planner_B.b_kstr++) {
2741 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
2742 =
2743 -cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr];
2744 }
2745
2746 for (cartesian_trajectory_planner_B.b_kstr = 0;
2747 cartesian_trajectory_planner_B.b_kstr < 3;
2748 cartesian_trajectory_planner_B.b_kstr++) {
2749 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
2750 << 2;
2751 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
2752 cartesian_trajectory_planner_B.R_g[3 *
2753 cartesian_trajectory_planner_B.b_kstr];
2754 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1] =
2755 cartesian_trajectory_planner_B.R_g[3 *
2756 cartesian_trajectory_planner_B.b_kstr + 1];
2757 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2] =
2758 cartesian_trajectory_planner_B.R_g[3 *
2759 cartesian_trajectory_planner_B.b_kstr + 2];
2760 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
2761 + 12] =
2762 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
2763 + 6] * cartesian_trajectory_planner_B.T1j[14] +
2764 (cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
2765 + 3] * cartesian_trajectory_planner_B.T1j[13] +
2766 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
2767 * cartesian_trajectory_planner_B.T1j[12]);
2768 }
2769
2770 cartesian_trajectory_planner_B.Tj[3] = 0.0;
2771 cartesian_trajectory_planner_B.Tj[7] = 0.0;
2772 cartesian_trajectory_planner_B.Tj[11] = 0.0;
2773 cartesian_trajectory_planner_B.Tj[15] = 1.0;
2774 cartesian_trajectory_planner_B.R_g[0] = 0.0;
2775 cartesian_trajectory_planner_B.R_g[3] =
2776 -cartesian_trajectory_planner_B.Tj[14];
2777 cartesian_trajectory_planner_B.R_g[6] = cartesian_trajectory_planner_B.Tj
2778 [13];
2779 cartesian_trajectory_planner_B.R_g[1] = cartesian_trajectory_planner_B.Tj
2780 [14];
2781 cartesian_trajectory_planner_B.R_g[4] = 0.0;
2782 cartesian_trajectory_planner_B.R_g[7] =
2783 -cartesian_trajectory_planner_B.Tj[12];
2784 cartesian_trajectory_planner_B.R_g[2] =
2785 -cartesian_trajectory_planner_B.Tj[13];
2786 cartesian_trajectory_planner_B.R_g[5] = cartesian_trajectory_planner_B.Tj
2787 [12];
2788 cartesian_trajectory_planner_B.R_g[8] = 0.0;
2789 for (cartesian_trajectory_planner_B.b_kstr = 0;
2790 cartesian_trajectory_planner_B.b_kstr < 3;
2791 cartesian_trajectory_planner_B.b_kstr++) {
2792 for (cartesian_trajectory_planner_B.g = 0;
2793 cartesian_trajectory_planner_B.g < 3;
2794 cartesian_trajectory_planner_B.g++) {
2795 cartesian_trajectory_planner_B.loop_ub_k =
2796 cartesian_trajectory_planner_B.b_kstr + 3 *
2797 cartesian_trajectory_planner_B.g;
2798 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.loop_ub_k]
2799 = 0.0;
2800 cartesian_trajectory_planner_B.n_l = cartesian_trajectory_planner_B.g <<
2801 2;
2802 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.loop_ub_k]
2803 +=
2804 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_l]
2805 * cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr];
2806 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.loop_ub_k]
2807 +=
2808 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_l
2809 + 1] *
2810 cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr
2811 + 3];
2812 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.loop_ub_k]
2813 +=
2814 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_l
2815 + 2] *
2816 cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr
2817 + 6];
2818 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
2819 cartesian_trajectory_planner_B.b_kstr] =
2820 cartesian_trajectory_planner_B.Tj
2821 [(cartesian_trajectory_planner_B.b_kstr << 2) +
2822 cartesian_trajectory_planner_B.g];
2823 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
2824 (cartesian_trajectory_planner_B.b_kstr + 3)] = 0.0;
2825 }
2826 }
2827
2828 for (cartesian_trajectory_planner_B.b_kstr = 0;
2829 cartesian_trajectory_planner_B.b_kstr < 3;
2830 cartesian_trajectory_planner_B.b_kstr++) {
2831 cartesian_trajectory_planner_B.X[6 *
2832 cartesian_trajectory_planner_B.b_kstr + 3] =
2833 cartesian_trajectory_planner_B.R_c[3 *
2834 cartesian_trajectory_planner_B.b_kstr];
2835 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
2836 << 2;
2837 cartesian_trajectory_planner_B.loop_ub_k = 6 *
2838 (cartesian_trajectory_planner_B.b_kstr + 3);
2839 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_k
2840 + 3] =
2841 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g];
2842 cartesian_trajectory_planner_B.X[6 *
2843 cartesian_trajectory_planner_B.b_kstr + 4] =
2844 cartesian_trajectory_planner_B.R_c[3 *
2845 cartesian_trajectory_planner_B.b_kstr + 1];
2846 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_k
2847 + 4] =
2848 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1];
2849 cartesian_trajectory_planner_B.X[6 *
2850 cartesian_trajectory_planner_B.b_kstr + 5] =
2851 cartesian_trajectory_planner_B.R_c[3 *
2852 cartesian_trajectory_planner_B.b_kstr + 2];
2853 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_k
2854 + 5] =
2855 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2];
2856 }
2857
2858 cartesian_trajectory_planner_B.b_kstr = b->size[0] * b->size[1];
2859 b->size[0] = 6;
2860 b->size[1] = joint->MotionSubspace->size[1];
2861 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr);
2862 cartesian_trajectory_planner_B.loop_ub_k = joint->MotionSubspace->size[0] *
2863 joint->MotionSubspace->size[1] - 1;
2864 for (cartesian_trajectory_planner_B.b_kstr = 0;
2865 cartesian_trajectory_planner_B.b_kstr <=
2866 cartesian_trajectory_planner_B.loop_ub_k;
2867 cartesian_trajectory_planner_B.b_kstr++) {
2868 b->data[cartesian_trajectory_planner_B.b_kstr] = joint->
2869 MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr];
2870 }
2871
2872 cartesian_trajectory_planner_B.n_l = b->size[1] - 1;
2873 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
2874 y->size[0] = 6;
2875 y->size[1] = b->size[1];
2876 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
2877 for (cartesian_trajectory_planner_B.b_kstr = 0;
2878 cartesian_trajectory_planner_B.b_kstr <=
2879 cartesian_trajectory_planner_B.n_l;
2880 cartesian_trajectory_planner_B.b_kstr++) {
2881 cartesian_trajectory_planner_B.coffset_tmp =
2882 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
2883 for (cartesian_trajectory_planner_B.g = 0;
2884 cartesian_trajectory_planner_B.g < 6;
2885 cartesian_trajectory_planner_B.g++) {
2886 cartesian_trajectory_planner_B.bid2 = 0.0;
2887 for (cartesian_trajectory_planner_B.loop_ub_k = 0;
2888 cartesian_trajectory_planner_B.loop_ub_k < 6;
2889 cartesian_trajectory_planner_B.loop_ub_k++) {
2890 cartesian_trajectory_planner_B.bid2 +=
2891 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_k
2892 * 6 + cartesian_trajectory_planner_B.g] * b->data
2893 [(cartesian_trajectory_planner_B.coffset_tmp +
2894 cartesian_trajectory_planner_B.loop_ub_k) + 1];
2895 }
2896
2897 y->data[(cartesian_trajectory_planner_B.coffset_tmp +
2898 cartesian_trajectory_planner_B.g) + 1] =
2899 cartesian_trajectory_planner_B.bid2;
2900 }
2901 }
2902
2903 if (cartesian_trajectory_planner_B.bid1 >
2904 cartesian_trajectory_planner_B.qidx_idx_1) {
2905 cartesian_trajectory_planner_B.n_l = 0;
2906 } else {
2907 cartesian_trajectory_planner_B.n_l = static_cast<int32_T>
2908 (cartesian_trajectory_planner_B.bid1) - 1;
2909 }
2910
2911 cartesian_trajectory_planner_B.loop_ub_k = y->size[1];
2912 for (cartesian_trajectory_planner_B.b_kstr = 0;
2913 cartesian_trajectory_planner_B.b_kstr <
2914 cartesian_trajectory_planner_B.loop_ub_k;
2915 cartesian_trajectory_planner_B.b_kstr++) {
2916 for (cartesian_trajectory_planner_B.g = 0;
2917 cartesian_trajectory_planner_B.g < 6;
2918 cartesian_trajectory_planner_B.g++) {
2919 Jac->data[cartesian_trajectory_planner_B.g + 6 *
2920 (cartesian_trajectory_planner_B.n_l +
2921 cartesian_trajectory_planner_B.b_kstr)] = y->data[6 *
2922 cartesian_trajectory_planner_B.b_kstr +
2923 cartesian_trajectory_planner_B.g] * static_cast<real_T>
2924 (cartesian_trajectory_planner_B.jointSign);
2925 }
2926 }
2927 }
2928
2929 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
2930 for (cartesian_trajectory_planner_B.b_kstr = 0;
2931 cartesian_trajectory_planner_B.b_kstr < 4;
2932 cartesian_trajectory_planner_B.b_kstr++) {
2933 for (cartesian_trajectory_planner_B.g = 0;
2934 cartesian_trajectory_planner_B.g < 4;
2935 cartesian_trajectory_planner_B.g++) {
2936 cartesian_trajectory_planner_B.loop_ub_k =
2937 cartesian_trajectory_planner_B.g << 2;
2938 cartesian_trajectory_planner_B.jointSign =
2939 cartesian_trajectory_planner_B.b_kstr +
2940 cartesian_trajectory_planner_B.loop_ub_k;
2941 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
2942 = 0.0;
2943 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
2944 +=
2945 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k]
2946 * cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
2947 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
2948 +=
2949 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2950 + 1] *
2951 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
2952 + 4];
2953 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
2954 +=
2955 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2956 + 2] *
2957 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
2958 + 8];
2959 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
2960 +=
2961 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_k
2962 + 3] *
2963 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
2964 + 12];
2965 }
2966 }
2967
2968 memcpy(&cartesian_trajectory_planner_B.T1[0],
2969 &cartesian_trajectory_planner_B.T1j[0], sizeof(real_T) << 4U);
2970 } else {
2971 for (cartesian_trajectory_planner_B.b_kstr = 0;
2972 cartesian_trajectory_planner_B.b_kstr < 3;
2973 cartesian_trajectory_planner_B.b_kstr++) {
2974 cartesian_trajectory_planner_B.R_g[3 *
2975 cartesian_trajectory_planner_B.b_kstr] =
2976 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
2977 cartesian_trajectory_planner_B.R_g[3 *
2978 cartesian_trajectory_planner_B.b_kstr + 1] =
2979 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
2980 + 4];
2981 cartesian_trajectory_planner_B.R_g[3 *
2982 cartesian_trajectory_planner_B.b_kstr + 2] =
2983 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
2984 + 8];
2985 }
2986
2987 for (cartesian_trajectory_planner_B.b_kstr = 0;
2988 cartesian_trajectory_planner_B.b_kstr < 9;
2989 cartesian_trajectory_planner_B.b_kstr++) {
2990 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
2991 =
2992 -cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.b_kstr];
2993 }
2994
2995 for (cartesian_trajectory_planner_B.b_kstr = 0;
2996 cartesian_trajectory_planner_B.b_kstr < 3;
2997 cartesian_trajectory_planner_B.b_kstr++) {
2998 cartesian_trajectory_planner_B.loop_ub_k =
2999 cartesian_trajectory_planner_B.b_kstr << 2;
3000 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_k]
3001 = cartesian_trajectory_planner_B.R_g[3 *
3002 cartesian_trajectory_planner_B.b_kstr];
3003 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_k
3004 + 1] = cartesian_trajectory_planner_B.R_g[3 *
3005 cartesian_trajectory_planner_B.b_kstr + 1];
3006 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_k
3007 + 2] = cartesian_trajectory_planner_B.R_g[3 *
3008 cartesian_trajectory_planner_B.b_kstr + 2];
3009 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3010 + 12] =
3011 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
3012 + 6] * cartesian_trajectory_planner_B.Tc2p[14] +
3013 (cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr
3014 + 3] * cartesian_trajectory_planner_B.Tc2p[13] +
3015 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr]
3016 * cartesian_trajectory_planner_B.Tc2p[12]);
3017 }
3018
3019 cartesian_trajectory_planner_B.T1j[3] = 0.0;
3020 cartesian_trajectory_planner_B.T1j[7] = 0.0;
3021 cartesian_trajectory_planner_B.T1j[11] = 0.0;
3022 cartesian_trajectory_planner_B.T1j[15] = 1.0;
3023 for (cartesian_trajectory_planner_B.b_kstr = 0;
3024 cartesian_trajectory_planner_B.b_kstr < 4;
3025 cartesian_trajectory_planner_B.b_kstr++) {
3026 for (cartesian_trajectory_planner_B.g = 0;
3027 cartesian_trajectory_planner_B.g < 4;
3028 cartesian_trajectory_planner_B.g++) {
3029 cartesian_trajectory_planner_B.jointSign =
3030 cartesian_trajectory_planner_B.g << 2;
3031 cartesian_trajectory_planner_B.loop_ub_k =
3032 cartesian_trajectory_planner_B.b_kstr +
3033 cartesian_trajectory_planner_B.jointSign;
3034 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_k]
3035 = 0.0;
3036 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_k]
3037 +=
3038 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign]
3039 * cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
3040 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_k]
3041 +=
3042 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3043 + 1] *
3044 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3045 + 4];
3046 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_k]
3047 +=
3048 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3049 + 2] *
3050 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3051 + 8];
3052 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_k]
3053 +=
3054 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3055 + 3] *
3056 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3057 + 12];
3058 }
3059 }
3060
3061 memcpy(&cartesian_trajectory_planner_B.T1[0],
3062 &cartesian_trajectory_planner_B.Tc2p[0], sizeof(real_T) << 4U);
3063 }
3064 }
3065
3066 cartesian_trajec_emxFree_real_T(&b);
3067 cartesian_trajec_emxFree_char_T(&body2Name);
3068 cartesian_trajec_emxFree_real_T(&kinematicPathIndices);
3069 for (cartesian_trajectory_planner_B.b_kstr = 0;
3070 cartesian_trajectory_planner_B.b_kstr < 3;
3071 cartesian_trajectory_planner_B.b_kstr++) {
3072 cartesian_trajectory_planner_B.b_i_d = cartesian_trajectory_planner_B.b_kstr
3073 << 2;
3074 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr] =
3075 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_d];
3076 cartesian_trajectory_planner_B.g = 6 *
3077 (cartesian_trajectory_planner_B.b_kstr + 3);
3078 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g] = 0.0;
3079 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3080 3] = 0.0;
3081 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 3] =
3082 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_d];
3083 cartesian_trajectory_planner_B.bid1 =
3084 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_d + 1];
3085 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3086 1] = cartesian_trajectory_planner_B.bid1;
3087 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 1] = 0.0;
3088 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3089 4] = 0.0;
3090 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 4] =
3091 cartesian_trajectory_planner_B.bid1;
3092 cartesian_trajectory_planner_B.bid1 =
3093 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_d + 2];
3094 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3095 2] = cartesian_trajectory_planner_B.bid1;
3096 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 2] = 0.0;
3097 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3098 5] = 0.0;
3099 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 5] =
3100 cartesian_trajectory_planner_B.bid1;
3101 }
3102
3103 cartesian_trajectory_planner_B.n_l = Jac->size[1];
3104 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
3105 y->size[0] = 6;
3106 y->size[1] = Jac->size[1];
3107 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
3108 cartesian_trajectory_planner_B.loop_ub_k = Jac->size[0] * Jac->size[1] - 1;
3109 for (cartesian_trajectory_planner_B.b_kstr = 0;
3110 cartesian_trajectory_planner_B.b_kstr <=
3111 cartesian_trajectory_planner_B.loop_ub_k;
3112 cartesian_trajectory_planner_B.b_kstr++) {
3113 y->data[cartesian_trajectory_planner_B.b_kstr] = Jac->
3114 data[cartesian_trajectory_planner_B.b_kstr];
3115 }
3116
3117 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
3118 Jac->size[0] = 6;
3119 Jac->size[1] = cartesian_trajectory_planner_B.n_l;
3120 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
3121 for (cartesian_trajectory_planner_B.b_kstr = 0;
3122 cartesian_trajectory_planner_B.b_kstr <
3123 cartesian_trajectory_planner_B.n_l; cartesian_trajectory_planner_B.b_kstr
3124 ++) {
3125 cartesian_trajectory_planner_B.coffset_tmp =
3126 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
3127 for (cartesian_trajectory_planner_B.b_i_d = 0;
3128 cartesian_trajectory_planner_B.b_i_d < 6;
3129 cartesian_trajectory_planner_B.b_i_d++) {
3130 cartesian_trajectory_planner_B.bid2 = 0.0;
3131 for (cartesian_trajectory_planner_B.loop_ub_k = 0;
3132 cartesian_trajectory_planner_B.loop_ub_k < 6;
3133 cartesian_trajectory_planner_B.loop_ub_k++) {
3134 cartesian_trajectory_planner_B.bid2 +=
3135 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_k
3136 * 6 + cartesian_trajectory_planner_B.b_i_d] * y->data
3137 [(cartesian_trajectory_planner_B.coffset_tmp +
3138 cartesian_trajectory_planner_B.loop_ub_k) + 1];
3139 }
3140
3141 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp +
3142 cartesian_trajectory_planner_B.b_i_d) + 1] =
3143 cartesian_trajectory_planner_B.bid2;
3144 }
3145 }
3146
3147 cartesian_trajec_emxFree_real_T(&y);
3148 T_size[0] = 4;
3149 T_size[1] = 4;
3150 memcpy(&T_data[0], &cartesian_trajectory_planner_B.T1[0], sizeof(real_T) << 4U);
3151}
3152
3153real_T rt_hypotd_snf(real_T u0, real_T u1)
3154{
3155 real_T y;
3156 real_T a;
3157 a = fabs(u0);
3158 y = fabs(u1);
3159 if (a < y) {
3160 a /= y;
3161 y *= sqrt(a * a + 1.0);
3162 } else if (a > y) {
3163 y /= a;
3164 y = sqrt(y * y + 1.0) * a;
3165 } else {
3166 if (!rtIsNaN(y)) {
3167 y = a * 1.4142135623730951;
3168 }
3169 }
3170
3171 return y;
3172}
3173
3174static creal_T cartesian_trajectory_plann_sqrt(const creal_T x)
3175{
3176 creal_T b_x;
3177 real_T absxr;
3178 real_T absxi;
3179 if (x.im == 0.0) {
3180 if (x.re < 0.0) {
3181 absxr = 0.0;
3182 absxi = sqrt(-x.re);
3183 } else {
3184 absxr = sqrt(x.re);
3185 absxi = 0.0;
3186 }
3187 } else if (x.re == 0.0) {
3188 if (x.im < 0.0) {
3189 absxr = sqrt(-x.im / 2.0);
3190 absxi = -absxr;
3191 } else {
3192 absxr = sqrt(x.im / 2.0);
3193 absxi = absxr;
3194 }
3195 } else if (rtIsNaN(x.re)) {
3196 absxr = x.re;
3197 absxi = x.re;
3198 } else if (rtIsNaN(x.im)) {
3199 absxr = x.im;
3200 absxi = x.im;
3201 } else if (rtIsInf(x.im)) {
3202 absxr = fabs(x.im);
3203 absxi = x.im;
3204 } else if (rtIsInf(x.re)) {
3205 if (x.re < 0.0) {
3206 absxr = 0.0;
3207 absxi = x.im * -x.re;
3208 } else {
3209 absxr = x.re;
3210 absxi = 0.0;
3211 }
3212 } else {
3213 absxr = fabs(x.re);
3214 absxi = fabs(x.im);
3215 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
3216 {
3217 absxr *= 0.5;
3218 absxi *= 0.5;
3219 absxi = rt_hypotd_snf(absxr, absxi);
3220 if (absxi > absxr) {
3221 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
3222 } else {
3223 absxr = sqrt(absxi) * 1.4142135623730951;
3224 }
3225 } else {
3226 absxr = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
3227 }
3228
3229 if (x.re > 0.0) {
3230 absxi = x.im / absxr * 0.5;
3231 } else {
3232 if (x.im < 0.0) {
3233 absxi = -absxr;
3234 } else {
3235 absxi = absxr;
3236 }
3237
3238 absxr = x.im / absxi * 0.5;
3239 }
3240 }
3241
3242 b_x.re = absxr;
3243 b_x.im = absxi;
3244 return b_x;
3245}
3246
3247real_T rt_atan2d_snf(real_T u0, real_T u1)
3248{
3249 real_T y;
3250 int32_T u0_0;
3251 int32_T u1_0;
3252 if (rtIsNaN(u0) || rtIsNaN(u1)) {
3253 y = (rtNaN);
3254 } else if (rtIsInf(u0) && rtIsInf(u1)) {
3255 if (u0 > 0.0) {
3256 u0_0 = 1;
3257 } else {
3258 u0_0 = -1;
3259 }
3260
3261 if (u1 > 0.0) {
3262 u1_0 = 1;
3263 } else {
3264 u1_0 = -1;
3265 }
3266
3267 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
3268 } else if (u1 == 0.0) {
3269 if (u0 > 0.0) {
3270 y = RT_PI / 2.0;
3271 } else if (u0 < 0.0) {
3272 y = -(RT_PI / 2.0);
3273 } else {
3274 y = 0.0;
3275 }
3276 } else {
3277 y = atan2(u0, u1);
3278 }
3279
3280 return y;
3281}
3282
3283static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
3284 int32_T ix0)
3285{
3286 real_T y;
3287 real_T scale;
3288 int32_T kend;
3289 real_T absxk;
3290 real_T t;
3291 int32_T k;
3292 y = 0.0;
3293 scale = 3.3121686421112381E-170;
3294 kend = ix0 + n;
3295 for (k = ix0; k < kend; k++) {
3296 absxk = fabs(x[k - 1]);
3297 if (absxk > scale) {
3298 t = scale / absxk;
3299 y = y * t * t + 1.0;
3300 scale = absxk;
3301 } else {
3302 t = absxk / scale;
3303 y += t * t;
3304 }
3305 }
3306
3307 return scale * sqrt(y);
3308}
3309
3310static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
3311 int32_T ix0, const real_T y[9], int32_T iy0)
3312{
3313 real_T d;
3314 int32_T ix;
3315 int32_T iy;
3316 int32_T k;
3317 d = 0.0;
3318 ix = ix0 - 1;
3319 iy = iy0 - 1;
3320 for (k = 0; k < n; k++) {
3321 d += x[ix] * y[iy];
3322 ix++;
3323 iy++;
3324 }
3325
3326 return d;
3327}
3328
3329static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
3330 const real_T y[9], int32_T iy0, real_T b_y[9])
3331{
3332 int32_T ix;
3333 int32_T iy;
3334 int32_T k;
3335 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3336 if (!(a == 0.0)) {
3337 ix = ix0;
3338 iy = iy0 - 1;
3339 for (k = 0; k < n; k++) {
3340 b_y[iy] += b_y[ix - 1] * a;
3341 ix++;
3342 iy++;
3343 }
3344 }
3345}
3346
3347static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0)
3348{
3349 real_T y;
3350 real_T scale;
3351 real_T absxk;
3352 real_T t;
3353 int32_T k;
3354 y = 0.0;
3355 scale = 3.3121686421112381E-170;
3356 for (k = ix0; k <= ix0 + 1; k++) {
3357 absxk = fabs(x[k - 1]);
3358 if (absxk > scale) {
3359 t = scale / absxk;
3360 y = y * t * t + 1.0;
3361 scale = absxk;
3362 } else {
3363 t = absxk / scale;
3364 y += t * t;
3365 }
3366 }
3367
3368 return scale * sqrt(y);
3369}
3370
3371static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
3372 [9], int32_T ix0, real_T y[3], int32_T iy0)
3373{
3374 int32_T ix;
3375 int32_T iy;
3376 int32_T k;
3377 if (!(a == 0.0)) {
3378 ix = ix0;
3379 iy = iy0 - 1;
3380 for (k = 0; k < n; k++) {
3381 y[iy] += x[ix - 1] * a;
3382 ix++;
3383 iy++;
3384 }
3385 }
3386}
3387
3388static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
3389 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
3390{
3391 int32_T ix;
3392 int32_T iy;
3393 int32_T k;
3394 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3395 if (!(a == 0.0)) {
3396 ix = ix0;
3397 iy = iy0 - 1;
3398 for (k = 0; k < n; k++) {
3399 b_y[iy] += x[ix - 1] * a;
3400 ix++;
3401 iy++;
3402 }
3403 }
3404}
3405
3406static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
3407 int32_T iy0, real_T b_x[9])
3408{
3409 int32_T ix;
3410 int32_T iy;
3411 real_T temp;
3412 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3413 ix = ix0 - 1;
3414 iy = iy0 - 1;
3415 temp = b_x[ix];
3416 b_x[ix] = b_x[iy];
3417 b_x[iy] = temp;
3418 ix++;
3419 iy++;
3420 temp = b_x[ix];
3421 b_x[ix] = b_x[iy];
3422 b_x[iy] = temp;
3423 ix++;
3424 iy++;
3425 temp = b_x[ix];
3426 b_x[ix] = b_x[iy];
3427 b_x[iy] = temp;
3428}
3429
3430static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
3431 real_T *b_b, real_T *c, real_T *s)
3432{
3433 cartesian_trajectory_planner_B.roe = b;
3434 cartesian_trajectory_planner_B.absa = fabs(a);
3435 cartesian_trajectory_planner_B.absb = fabs(b);
3436 if (cartesian_trajectory_planner_B.absa > cartesian_trajectory_planner_B.absb)
3437 {
3438 cartesian_trajectory_planner_B.roe = a;
3439 }
3440
3441 cartesian_trajectory_planner_B.scale = cartesian_trajectory_planner_B.absa +
3442 cartesian_trajectory_planner_B.absb;
3443 if (cartesian_trajectory_planner_B.scale == 0.0) {
3444 *s = 0.0;
3445 *c = 1.0;
3446 *b_a = 0.0;
3447 *b_b = 0.0;
3448 } else {
3449 cartesian_trajectory_planner_B.ads = cartesian_trajectory_planner_B.absa /
3450 cartesian_trajectory_planner_B.scale;
3451 cartesian_trajectory_planner_B.bds = cartesian_trajectory_planner_B.absb /
3452 cartesian_trajectory_planner_B.scale;
3453 *b_a = sqrt(cartesian_trajectory_planner_B.ads *
3454 cartesian_trajectory_planner_B.ads +
3455 cartesian_trajectory_planner_B.bds *
3456 cartesian_trajectory_planner_B.bds) *
3457 cartesian_trajectory_planner_B.scale;
3458 if (cartesian_trajectory_planner_B.roe < 0.0) {
3459 *b_a = -*b_a;
3460 }
3461
3462 *c = a / *b_a;
3463 *s = b / *b_a;
3464 if (cartesian_trajectory_planner_B.absa >
3465 cartesian_trajectory_planner_B.absb) {
3466 *b_b = *s;
3467 } else if (*c != 0.0) {
3468 *b_b = 1.0 / *c;
3469 } else {
3470 *b_b = 1.0;
3471 }
3472 }
3473}
3474
3475static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
3476 int32_T iy0, real_T c, real_T s, real_T b_x[9])
3477{
3478 int32_T ix;
3479 int32_T iy;
3480 real_T temp;
3481 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3482 ix = ix0 - 1;
3483 iy = iy0 - 1;
3484 temp = c * b_x[ix] + s * b_x[iy];
3485 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3486 b_x[ix] = temp;
3487 iy++;
3488 ix++;
3489 temp = c * b_x[ix] + s * b_x[iy];
3490 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3491 b_x[ix] = temp;
3492 iy++;
3493 ix++;
3494 temp = c * b_x[ix] + s * b_x[iy];
3495 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3496 b_x[ix] = temp;
3497}
3498
3499static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
3500 real_T s[3], real_T V[9])
3501{
3502 int32_T qq;
3503 boolean_T apply_transform;
3504 int32_T qjj;
3505 int32_T m;
3506 int32_T kase;
3507 int32_T c_q;
3508 int32_T d_k;
3509 boolean_T exitg1;
3510 cartesian_trajectory_planner_B.e_d[0] = 0.0;
3511 cartesian_trajectory_planner_B.work[0] = 0.0;
3512 cartesian_trajectory_planner_B.e_d[1] = 0.0;
3513 cartesian_trajectory_planner_B.work[1] = 0.0;
3514 cartesian_trajectory_planner_B.e_d[2] = 0.0;
3515 cartesian_trajectory_planner_B.work[2] = 0.0;
3516 for (m = 0; m < 9; m++) {
3517 cartesian_trajectory_planner_B.A[m] = A[m];
3518 U[m] = 0.0;
3519 V[m] = 0.0;
3520 }
3521
3522 apply_transform = false;
3523 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(3,
3524 cartesian_trajectory_planner_B.A, 1);
3525 if (cartesian_trajectory_planner_B.nrm > 0.0) {
3526 apply_transform = true;
3527 if (cartesian_trajectory_planner_B.A[0] < 0.0) {
3528 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.nrm;
3529 } else {
3530 cartesian_trajectory_planner_B.s[0] = cartesian_trajectory_planner_B.nrm;
3531 }
3532
3533 if (fabs(cartesian_trajectory_planner_B.s[0]) >= 1.0020841800044864E-292) {
3534 cartesian_trajectory_planner_B.nrm = 1.0 /
3535 cartesian_trajectory_planner_B.s[0];
3536 for (qq = 1; qq < 4; qq++) {
3537 cartesian_trajectory_planner_B.A[qq - 1] *=
3538 cartesian_trajectory_planner_B.nrm;
3539 }
3540 } else {
3541 for (qq = 1; qq < 4; qq++) {
3542 cartesian_trajectory_planner_B.A[qq - 1] /=
3543 cartesian_trajectory_planner_B.s[0];
3544 }
3545 }
3546
3547 cartesian_trajectory_planner_B.A[0]++;
3548 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.s[0];
3549 } else {
3550 cartesian_trajectory_planner_B.s[0] = 0.0;
3551 }
3552
3553 for (m = 2; m < 4; m++) {
3554 qjj = (m - 1) * 3 + 1;
3555 if (apply_transform) {
3556 memcpy(&cartesian_trajectory_planner_B.A_h[0],
3557 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
3558 cartesian_trajectory_plan_xaxpy(3, -(cartesian_trajectory_plan_xdotc(3,
3559 cartesian_trajectory_planner_B.A, 1, cartesian_trajectory_planner_B.A,
3560 qjj) / cartesian_trajectory_planner_B.A[0]), 1,
3561 cartesian_trajectory_planner_B.A_h, qjj,
3562 cartesian_trajectory_planner_B.A);
3563 }
3564
3565 cartesian_trajectory_planner_B.e_d[m - 1] =
3566 cartesian_trajectory_planner_B.A[qjj - 1];
3567 }
3568
3569 for (m = 1; m < 4; m++) {
3570 U[m - 1] = cartesian_trajectory_planner_B.A[m - 1];
3571 }
3572
3573 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_pl_xnrm2_a
3574 (cartesian_trajectory_planner_B.e_d, 2);
3575 if (cartesian_trajectory_planner_B.nrm == 0.0) {
3576 cartesian_trajectory_planner_B.e_d[0] = 0.0;
3577 } else {
3578 if (cartesian_trajectory_planner_B.e_d[1] < 0.0) {
3579 cartesian_trajectory_planner_B.rt = -cartesian_trajectory_planner_B.nrm;
3580 cartesian_trajectory_planner_B.e_d[0] =
3581 -cartesian_trajectory_planner_B.nrm;
3582 } else {
3583 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.nrm;
3584 cartesian_trajectory_planner_B.e_d[0] = cartesian_trajectory_planner_B.nrm;
3585 }
3586
3587 if (fabs(cartesian_trajectory_planner_B.rt) >= 1.0020841800044864E-292) {
3588 cartesian_trajectory_planner_B.nrm = 1.0 /
3589 cartesian_trajectory_planner_B.rt;
3590 for (qq = 2; qq < 4; qq++) {
3591 cartesian_trajectory_planner_B.e_d[qq - 1] *=
3592 cartesian_trajectory_planner_B.nrm;
3593 }
3594 } else {
3595 for (qq = 2; qq < 4; qq++) {
3596 cartesian_trajectory_planner_B.e_d[qq - 1] /=
3597 cartesian_trajectory_planner_B.rt;
3598 }
3599 }
3600
3601 cartesian_trajectory_planner_B.e_d[1]++;
3602 cartesian_trajectory_planner_B.e_d[0] = -cartesian_trajectory_planner_B.e_d
3603 [0];
3604 for (m = 2; m < 4; m++) {
3605 cartesian_trajectory_planner_B.work[m - 1] = 0.0;
3606 }
3607
3608 for (m = 2; m < 4; m++) {
3609 cartesian_trajectory__xaxpy_ast(2, cartesian_trajectory_planner_B.e_d[m -
3610 1], cartesian_trajectory_planner_B.A, 3 * (m - 1) + 2,
3611 cartesian_trajectory_planner_B.work, 2);
3612 }
3613
3614 for (m = 2; m < 4; m++) {
3615 memcpy(&cartesian_trajectory_planner_B.A_h[0],
3616 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
3617 cartesian_trajectory_p_xaxpy_as(2, -cartesian_trajectory_planner_B.e_d[m -
3618 1] / cartesian_trajectory_planner_B.e_d[1],
3619 cartesian_trajectory_planner_B.work, 2,
3620 cartesian_trajectory_planner_B.A_h, (m - 1) * 3 + 2,
3621 cartesian_trajectory_planner_B.A);
3622 }
3623 }
3624
3625 for (m = 2; m < 4; m++) {
3626 V[m - 1] = cartesian_trajectory_planner_B.e_d[m - 1];
3627 }
3628
3629 apply_transform = false;
3630 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(2,
3631 cartesian_trajectory_planner_B.A, 5);
3632 if (cartesian_trajectory_planner_B.nrm > 0.0) {
3633 apply_transform = true;
3634 if (cartesian_trajectory_planner_B.A[4] < 0.0) {
3635 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.nrm;
3636 } else {
3637 cartesian_trajectory_planner_B.s[1] = cartesian_trajectory_planner_B.nrm;
3638 }
3639
3640 if (fabs(cartesian_trajectory_planner_B.s[1]) >= 1.0020841800044864E-292) {
3641 cartesian_trajectory_planner_B.nrm = 1.0 /
3642 cartesian_trajectory_planner_B.s[1];
3643 for (qq = 5; qq < 7; qq++) {
3644 cartesian_trajectory_planner_B.A[qq - 1] *=
3645 cartesian_trajectory_planner_B.nrm;
3646 }
3647 } else {
3648 for (qq = 5; qq < 7; qq++) {
3649 cartesian_trajectory_planner_B.A[qq - 1] /=
3650 cartesian_trajectory_planner_B.s[1];
3651 }
3652 }
3653
3654 cartesian_trajectory_planner_B.A[4]++;
3655 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.s[1];
3656 } else {
3657 cartesian_trajectory_planner_B.s[1] = 0.0;
3658 }
3659
3660 if (apply_transform) {
3661 for (m = 3; m < 4; m++) {
3662 memcpy(&cartesian_trajectory_planner_B.A_h[0],
3663 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
3664 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2,
3665 cartesian_trajectory_planner_B.A, 5, cartesian_trajectory_planner_B.A, 8)
3666 / cartesian_trajectory_planner_B.A[4]), 5,
3667 cartesian_trajectory_planner_B.A_h, 8, cartesian_trajectory_planner_B.A);
3668 }
3669 }
3670
3671 for (m = 2; m < 4; m++) {
3672 U[m + 2] = cartesian_trajectory_planner_B.A[m + 2];
3673 }
3674
3675 m = 2;
3676 cartesian_trajectory_planner_B.s[2] = cartesian_trajectory_planner_B.A[8];
3677 cartesian_trajectory_planner_B.e_d[1] = cartesian_trajectory_planner_B.A[7];
3678 cartesian_trajectory_planner_B.e_d[2] = 0.0;
3679 U[6] = 0.0;
3680 U[7] = 0.0;
3681 U[8] = 1.0;
3682 for (c_q = 1; c_q >= 0; c_q--) {
3683 qq = 3 * c_q + c_q;
3684 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
3685 for (kase = c_q + 2; kase < 4; kase++) {
3686 qjj = ((kase - 1) * 3 + c_q) + 1;
3687 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3688 cartesian_trajectory_plan_xaxpy(3 - c_q,
3689 -(cartesian_trajectory_plan_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
3690 qq + 1, cartesian_trajectory_planner_B.A, qjj, U);
3691 }
3692
3693 for (qjj = c_q + 1; qjj < 4; qjj++) {
3694 kase = (3 * c_q + qjj) - 1;
3695 U[kase] = -U[kase];
3696 }
3697
3698 U[qq]++;
3699 if (0 <= c_q - 1) {
3700 U[3 * c_q] = 0.0;
3701 }
3702 } else {
3703 U[3 * c_q] = 0.0;
3704 U[3 * c_q + 1] = 0.0;
3705 U[3 * c_q + 2] = 0.0;
3706 U[qq] = 1.0;
3707 }
3708 }
3709
3710 for (c_q = 2; c_q >= 0; c_q--) {
3711 if ((c_q + 1 <= 1) && (cartesian_trajectory_planner_B.e_d[0] != 0.0)) {
3712 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3713 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
3714 2, V, 5) / V[1]), 2, cartesian_trajectory_planner_B.A, 5, V);
3715 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3716 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
3717 2, V, 8) / V[1]), 2, cartesian_trajectory_planner_B.A, 8, V);
3718 }
3719
3720 V[3 * c_q] = 0.0;
3721 V[3 * c_q + 1] = 0.0;
3722 V[3 * c_q + 2] = 0.0;
3723 V[c_q + 3 * c_q] = 1.0;
3724 }
3725
3726 for (c_q = 0; c_q < 3; c_q++) {
3727 cartesian_trajectory_planner_B.ztest =
3728 cartesian_trajectory_planner_B.e_d[c_q];
3729 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
3730 cartesian_trajectory_planner_B.rt = fabs
3731 (cartesian_trajectory_planner_B.s[c_q]);
3732 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.s[c_q]
3733 / cartesian_trajectory_planner_B.rt;
3734 cartesian_trajectory_planner_B.s[c_q] = cartesian_trajectory_planner_B.rt;
3735 if (c_q + 1 < 3) {
3736 cartesian_trajectory_planner_B.ztest =
3737 cartesian_trajectory_planner_B.e_d[c_q] /
3738 cartesian_trajectory_planner_B.nrm;
3739 }
3740
3741 qjj = 3 * c_q;
3742 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3743 U[qq - 1] *= cartesian_trajectory_planner_B.nrm;
3744 }
3745 }
3746
3747 if ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.ztest != 0.0)) {
3748 cartesian_trajectory_planner_B.rt = fabs
3749 (cartesian_trajectory_planner_B.ztest);
3750 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt /
3751 cartesian_trajectory_planner_B.ztest;
3752 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
3753 cartesian_trajectory_planner_B.s[c_q + 1] *=
3754 cartesian_trajectory_planner_B.nrm;
3755 qjj = (c_q + 1) * 3;
3756 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3757 V[qq - 1] *= cartesian_trajectory_planner_B.nrm;
3758 }
3759 }
3760
3761 cartesian_trajectory_planner_B.e_d[c_q] =
3762 cartesian_trajectory_planner_B.ztest;
3763 }
3764
3765 qq = 0;
3766 cartesian_trajectory_planner_B.nrm = 0.0;
3767 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[0]);
3768 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_d[0]);
3769 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
3770 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
3771 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
3772 }
3773
3774 if (!rtIsNaN(cartesian_trajectory_planner_B.rt)) {
3775 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
3776 }
3777
3778 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[1]);
3779 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_d[1]);
3780 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
3781 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
3782 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
3783 }
3784
3785 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
3786 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
3787 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
3788 }
3789
3790 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[2]);
3791 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_d[2]);
3792 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
3793 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
3794 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
3795 }
3796
3797 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
3798 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
3799 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
3800 }
3801
3802 while ((m + 1 > 0) && (!(qq >= 75))) {
3803 c_q = m;
3804 qjj = m;
3805 exitg1 = false;
3806 while ((!exitg1) && (qjj > -1)) {
3807 c_q = qjj;
3808 if (qjj == 0) {
3809 exitg1 = true;
3810 } else {
3811 cartesian_trajectory_planner_B.rt = fabs
3812 (cartesian_trajectory_planner_B.e_d[qjj - 1]);
3813 if ((cartesian_trajectory_planner_B.rt <= (fabs
3814 (cartesian_trajectory_planner_B.s[qjj - 1]) + fabs
3815 (cartesian_trajectory_planner_B.s[qjj])) * 2.2204460492503131E-16)
3816 || (cartesian_trajectory_planner_B.rt <= 1.0020841800044864E-292) ||
3817 ((qq > 20) && (cartesian_trajectory_planner_B.rt <=
3818 2.2204460492503131E-16 *
3819 cartesian_trajectory_planner_B.nrm))) {
3820 cartesian_trajectory_planner_B.e_d[qjj - 1] = 0.0;
3821 exitg1 = true;
3822 } else {
3823 qjj--;
3824 }
3825 }
3826 }
3827
3828 if (c_q == m) {
3829 kase = 4;
3830 } else {
3831 qjj = m + 1;
3832 kase = m + 1;
3833 exitg1 = false;
3834 while ((!exitg1) && (kase >= c_q)) {
3835 qjj = kase;
3836 if (kase == c_q) {
3837 exitg1 = true;
3838 } else {
3839 cartesian_trajectory_planner_B.rt = 0.0;
3840 if (kase < m + 1) {
3841 cartesian_trajectory_planner_B.rt = fabs
3842 (cartesian_trajectory_planner_B.e_d[kase - 1]);
3843 }
3844
3845 if (kase > c_q + 1) {
3846 cartesian_trajectory_planner_B.rt += fabs
3847 (cartesian_trajectory_planner_B.e_d[kase - 2]);
3848 }
3849
3850 cartesian_trajectory_planner_B.ztest = fabs
3851 (cartesian_trajectory_planner_B.s[kase - 1]);
3852 if ((cartesian_trajectory_planner_B.ztest <= 2.2204460492503131E-16 *
3853 cartesian_trajectory_planner_B.rt) ||
3854 (cartesian_trajectory_planner_B.ztest <= 1.0020841800044864E-292))
3855 {
3856 cartesian_trajectory_planner_B.s[kase - 1] = 0.0;
3857 exitg1 = true;
3858 } else {
3859 kase--;
3860 }
3861 }
3862 }
3863
3864 if (qjj == c_q) {
3865 kase = 3;
3866 } else if (m + 1 == qjj) {
3867 kase = 1;
3868 } else {
3869 kase = 2;
3870 c_q = qjj;
3871 }
3872 }
3873
3874 switch (kase) {
3875 case 1:
3876 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_d[m -
3877 1];
3878 cartesian_trajectory_planner_B.e_d[m - 1] = 0.0;
3879 for (qjj = m; qjj >= c_q + 1; qjj--) {
3880 cartesian_trajectory_planner_B.ztest =
3881 cartesian_trajectory_planner_B.e_d[0];
3882 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
3883 cartesian_trajectory_planner_B.rt,
3884 &cartesian_trajectory_planner_B.s[qjj - 1],
3885 &cartesian_trajectory_planner_B.rt,
3886 &cartesian_trajectory_planner_B.sqds,
3887 &cartesian_trajectory_planner_B.b_f);
3888 if (qjj > c_q + 1) {
3889 cartesian_trajectory_planner_B.rt =
3890 -cartesian_trajectory_planner_B.b_f *
3891 cartesian_trajectory_planner_B.e_d[0];
3892 cartesian_trajectory_planner_B.ztest =
3893 cartesian_trajectory_planner_B.e_d[0] *
3894 cartesian_trajectory_planner_B.sqds;
3895 }
3896
3897 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3898 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
3899 1) * 3 + 1, 3 * m + 1, cartesian_trajectory_planner_B.sqds,
3900 cartesian_trajectory_planner_B.b_f, V);
3901 cartesian_trajectory_planner_B.e_d[0] =
3902 cartesian_trajectory_planner_B.ztest;
3903 }
3904 break;
3905
3906 case 2:
3907 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_d[c_q
3908 - 1];
3909 cartesian_trajectory_planner_B.e_d[c_q - 1] = 0.0;
3910 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
3911 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
3912 cartesian_trajectory_planner_B.rt,
3913 &cartesian_trajectory_planner_B.s[qjj - 1],
3914 &cartesian_trajectory_planner_B.ztest,
3915 &cartesian_trajectory_planner_B.sqds,
3916 &cartesian_trajectory_planner_B.b_f);
3917 cartesian_trajectory_planner_B.ztest =
3918 cartesian_trajectory_planner_B.e_d[qjj - 1];
3919 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest
3920 * -cartesian_trajectory_planner_B.b_f;
3921 cartesian_trajectory_planner_B.e_d[qjj - 1] =
3922 cartesian_trajectory_planner_B.ztest *
3923 cartesian_trajectory_planner_B.sqds;
3924 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3925 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
3926 1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_trajectory_planner_B.sqds,
3927 cartesian_trajectory_planner_B.b_f, U);
3928 }
3929 break;
3930
3931 case 3:
3932 cartesian_trajectory_planner_B.ztest = fabs
3933 (cartesian_trajectory_planner_B.s[m]);
3934 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[m -
3935 1];
3936 cartesian_trajectory_planner_B.rt = fabs
3937 (cartesian_trajectory_planner_B.sqds);
3938 if ((cartesian_trajectory_planner_B.ztest >
3939 cartesian_trajectory_planner_B.rt) || rtIsNaN
3940 (cartesian_trajectory_planner_B.rt)) {
3941 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
3942 }
3943
3944 cartesian_trajectory_planner_B.b_f = cartesian_trajectory_planner_B.e_d[m
3945 - 1];
3946 cartesian_trajectory_planner_B.ztest = fabs
3947 (cartesian_trajectory_planner_B.b_f);
3948 if ((cartesian_trajectory_planner_B.rt >
3949 cartesian_trajectory_planner_B.ztest) || rtIsNaN
3950 (cartesian_trajectory_planner_B.ztest)) {
3951 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
3952 }
3953
3954 cartesian_trajectory_planner_B.rt = fabs
3955 (cartesian_trajectory_planner_B.s[c_q]);
3956 if ((cartesian_trajectory_planner_B.ztest >
3957 cartesian_trajectory_planner_B.rt) || rtIsNaN
3958 (cartesian_trajectory_planner_B.rt)) {
3959 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
3960 }
3961
3962 cartesian_trajectory_planner_B.ztest = fabs
3963 (cartesian_trajectory_planner_B.e_d[c_q]);
3964 if ((cartesian_trajectory_planner_B.rt >
3965 cartesian_trajectory_planner_B.ztest) || rtIsNaN
3966 (cartesian_trajectory_planner_B.ztest)) {
3967 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
3968 }
3969
3970 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[m] /
3971 cartesian_trajectory_planner_B.ztest;
3972 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.sqds /
3973 cartesian_trajectory_planner_B.ztest;
3974 cartesian_trajectory_planner_B.emm1 = cartesian_trajectory_planner_B.b_f /
3975 cartesian_trajectory_planner_B.ztest;
3976 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[c_q]
3977 / cartesian_trajectory_planner_B.ztest;
3978 cartesian_trajectory_planner_B.b_f = ((cartesian_trajectory_planner_B.smm1
3979 + cartesian_trajectory_planner_B.rt) *
3980 (cartesian_trajectory_planner_B.smm1 - cartesian_trajectory_planner_B.rt)
3981 + cartesian_trajectory_planner_B.emm1 *
3982 cartesian_trajectory_planner_B.emm1) / 2.0;
3983 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.rt *
3984 cartesian_trajectory_planner_B.emm1;
3985 cartesian_trajectory_planner_B.smm1 *= cartesian_trajectory_planner_B.smm1;
3986 if ((cartesian_trajectory_planner_B.b_f != 0.0) ||
3987 (cartesian_trajectory_planner_B.smm1 != 0.0)) {
3988 cartesian_trajectory_planner_B.emm1 = sqrt
3989 (cartesian_trajectory_planner_B.b_f *
3990 cartesian_trajectory_planner_B.b_f +
3991 cartesian_trajectory_planner_B.smm1);
3992 if (cartesian_trajectory_planner_B.b_f < 0.0) {
3993 cartesian_trajectory_planner_B.emm1 =
3994 -cartesian_trajectory_planner_B.emm1;
3995 }
3996
3997 cartesian_trajectory_planner_B.emm1 =
3998 cartesian_trajectory_planner_B.smm1 /
3999 (cartesian_trajectory_planner_B.b_f +
4000 cartesian_trajectory_planner_B.emm1);
4001 } else {
4002 cartesian_trajectory_planner_B.emm1 = 0.0;
4003 }
4004
4005 cartesian_trajectory_planner_B.rt = (cartesian_trajectory_planner_B.sqds +
4006 cartesian_trajectory_planner_B.rt) *
4007 (cartesian_trajectory_planner_B.sqds - cartesian_trajectory_planner_B.rt)
4008 + cartesian_trajectory_planner_B.emm1;
4009 cartesian_trajectory_planner_B.sqds *=
4010 cartesian_trajectory_planner_B.e_d[c_q] /
4011 cartesian_trajectory_planner_B.ztest;
4012 for (d_k = c_q + 1; d_k <= m; d_k++) {
4013 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt,
4014 cartesian_trajectory_planner_B.sqds,
4015 &cartesian_trajectory_planner_B.ztest,
4016 &cartesian_trajectory_planner_B.emm1,
4017 &cartesian_trajectory_planner_B.b_f,
4018 &cartesian_trajectory_planner_B.smm1);
4019 if (d_k > c_q + 1) {
4020 cartesian_trajectory_planner_B.e_d[0] =
4021 cartesian_trajectory_planner_B.ztest;
4022 }
4023
4024 cartesian_trajectory_planner_B.ztest =
4025 cartesian_trajectory_planner_B.e_d[d_k - 1];
4026 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[d_k
4027 - 1];
4028 cartesian_trajectory_planner_B.e_d[d_k - 1] =
4029 cartesian_trajectory_planner_B.ztest *
4030 cartesian_trajectory_planner_B.b_f - cartesian_trajectory_planner_B.rt
4031 * cartesian_trajectory_planner_B.smm1;
4032 cartesian_trajectory_planner_B.sqds =
4033 cartesian_trajectory_planner_B.smm1 *
4034 cartesian_trajectory_planner_B.s[d_k];
4035 cartesian_trajectory_planner_B.s[d_k] *=
4036 cartesian_trajectory_planner_B.b_f;
4037 qjj = (d_k - 1) * 3 + 1;
4038 kase = 3 * d_k + 1;
4039 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4040 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
4041 kase, cartesian_trajectory_planner_B.b_f,
4042 cartesian_trajectory_planner_B.smm1, V);
4043 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt *
4044 cartesian_trajectory_planner_B.b_f +
4045 cartesian_trajectory_planner_B.ztest *
4046 cartesian_trajectory_planner_B.smm1,
4047 cartesian_trajectory_planner_B.sqds,
4048 &cartesian_trajectory_planner_B.s[d_k - 1],
4049 &cartesian_trajectory_planner_B.unusedU2,
4050 &cartesian_trajectory_planner_B.emm1,
4051 &cartesian_trajectory_planner_B.d_sn);
4052 cartesian_trajectory_planner_B.rt =
4053 cartesian_trajectory_planner_B.e_d[d_k - 1] *
4054 cartesian_trajectory_planner_B.emm1 +
4055 cartesian_trajectory_planner_B.d_sn *
4056 cartesian_trajectory_planner_B.s[d_k];
4057 cartesian_trajectory_planner_B.s[d_k] =
4058 cartesian_trajectory_planner_B.e_d[d_k - 1] *
4059 -cartesian_trajectory_planner_B.d_sn +
4060 cartesian_trajectory_planner_B.emm1 *
4061 cartesian_trajectory_planner_B.s[d_k];
4062 cartesian_trajectory_planner_B.sqds =
4063 cartesian_trajectory_planner_B.d_sn *
4064 cartesian_trajectory_planner_B.e_d[d_k];
4065 cartesian_trajectory_planner_B.e_d[d_k] *=
4066 cartesian_trajectory_planner_B.emm1;
4067 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4068 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
4069 kase, cartesian_trajectory_planner_B.emm1,
4070 cartesian_trajectory_planner_B.d_sn, U);
4071 }
4072
4073 cartesian_trajectory_planner_B.e_d[m - 1] =
4074 cartesian_trajectory_planner_B.rt;
4075 qq++;
4076 break;
4077
4078 default:
4079 if (cartesian_trajectory_planner_B.s[c_q] < 0.0) {
4080 cartesian_trajectory_planner_B.s[c_q] =
4081 -cartesian_trajectory_planner_B.s[c_q];
4082 qjj = 3 * c_q;
4083 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
4084 V[qq - 1] = -V[qq - 1];
4085 }
4086 }
4087
4088 qq = c_q + 1;
4089 while ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.s[c_q] <
4090 cartesian_trajectory_planner_B.s[qq])) {
4091 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[c_q];
4092 cartesian_trajectory_planner_B.s[c_q] =
4093 cartesian_trajectory_planner_B.s[qq];
4094 cartesian_trajectory_planner_B.s[qq] = cartesian_trajectory_planner_B.rt;
4095 qjj = 3 * c_q + 1;
4096 kase = (c_q + 1) * 3 + 1;
4097 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4098 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
4099 kase, V);
4100 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4101 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
4102 kase, U);
4103 c_q = qq;
4104 qq++;
4105 }
4106
4107 qq = 0;
4108 m--;
4109 break;
4110 }
4111 }
4112
4113 s[0] = cartesian_trajectory_planner_B.s[0];
4114 s[1] = cartesian_trajectory_planner_B.s[1];
4115 s[2] = cartesian_trajectory_planner_B.s[2];
4116}
4117
4118static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4])
4119{
4120 boolean_T e;
4121 boolean_T p;
4122 boolean_T rEQ0;
4123 int32_T loop_ub_tmp;
4124 boolean_T exitg1;
4125 cartesian_trajectory_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
4126 if (!(fabs(cartesian_trajectory_planner_B.u.re) > 1.0)) {
4127 cartesian_trajectory_planner_B.v_fx.re = acos
4128 (cartesian_trajectory_planner_B.u.re);
4129 } else {
4130 cartesian_trajectory_planner_B.u_p.re = cartesian_trajectory_planner_B.u.re
4131 + 1.0;
4132 cartesian_trajectory_planner_B.u_p.im = 0.0;
4133 cartesian_trajectory_planner_B.dc.re = 1.0 -
4134 cartesian_trajectory_planner_B.u.re;
4135 cartesian_trajectory_planner_B.dc.im = 0.0;
4136 cartesian_trajectory_planner_B.v_fx.re = 2.0 * rt_atan2d_snf
4137 ((cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.dc)).re,
4138 (cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.u_p)).re);
4139 }
4140
4141 cartesian_trajectory_planner_B.a_cm = 2.0 * sin
4142 (cartesian_trajectory_planner_B.v_fx.re);
4143 cartesian_trajectory_planner_B.v_nr[0] = (R[5] - R[7]) /
4144 cartesian_trajectory_planner_B.a_cm;
4145 cartesian_trajectory_planner_B.v_nr[1] = (R[6] - R[2]) /
4146 cartesian_trajectory_planner_B.a_cm;
4147 cartesian_trajectory_planner_B.v_nr[2] = (R[1] - R[3]) /
4148 cartesian_trajectory_planner_B.a_cm;
4149 if (rtIsNaN(cartesian_trajectory_planner_B.v_fx.re) || rtIsInf
4150 (cartesian_trajectory_planner_B.v_fx.re)) {
4151 cartesian_trajectory_planner_B.a_cm = (rtNaN);
4152 } else if (cartesian_trajectory_planner_B.v_fx.re == 0.0) {
4153 cartesian_trajectory_planner_B.a_cm = 0.0;
4154 } else {
4155 cartesian_trajectory_planner_B.a_cm = fmod
4156 (cartesian_trajectory_planner_B.v_fx.re, 3.1415926535897931);
4157 rEQ0 = (cartesian_trajectory_planner_B.a_cm == 0.0);
4158 if (!rEQ0) {
4159 cartesian_trajectory_planner_B.q = fabs
4160 (cartesian_trajectory_planner_B.v_fx.re / 3.1415926535897931);
4161 rEQ0 = !(fabs(cartesian_trajectory_planner_B.q - floor
4162 (cartesian_trajectory_planner_B.q + 0.5)) >
4163 2.2204460492503131E-16 * cartesian_trajectory_planner_B.q);
4164 }
4165
4166 if (rEQ0) {
4167 cartesian_trajectory_planner_B.a_cm = 0.0;
4168 } else {
4169 if (cartesian_trajectory_planner_B.v_fx.re < 0.0) {
4170 cartesian_trajectory_planner_B.a_cm += 3.1415926535897931;
4171 }
4172 }
4173 }
4174
4175 rEQ0 = (cartesian_trajectory_planner_B.a_cm == 0.0);
4176 e = true;
4177 cartesian_trajectory_planner_B.b_k_p = 0;
4178 exitg1 = false;
4179 while ((!exitg1) && (cartesian_trajectory_planner_B.b_k_p < 3)) {
4180 if (!(cartesian_trajectory_planner_B.v_nr[cartesian_trajectory_planner_B.b_k_p]
4181 == 0.0)) {
4182 e = false;
4183 exitg1 = true;
4184 } else {
4185 cartesian_trajectory_planner_B.b_k_p++;
4186 }
4187 }
4188
4189 if (rEQ0 || e) {
4190 loop_ub_tmp = (rEQ0 || e);
4191 cartesian_trajectory_planner_B.loop_ub_nk = loop_ub_tmp * 3 - 1;
4192 if (0 <= cartesian_trajectory_planner_B.loop_ub_nk) {
4193 memset(&cartesian_trajectory_planner_B.vspecial_data[0], 0,
4194 (cartesian_trajectory_planner_B.loop_ub_nk + 1) * sizeof(real_T));
4195 }
4196
4197 loop_ub_tmp--;
4198 for (cartesian_trajectory_planner_B.loop_ub_nk = 0;
4199 cartesian_trajectory_planner_B.loop_ub_nk <= loop_ub_tmp;
4200 cartesian_trajectory_planner_B.loop_ub_nk++) {
4201 memset(&cartesian_trajectory_planner_B.b_I[0], 0, 9U * sizeof(real_T));
4202 cartesian_trajectory_planner_B.b_I[0] = 1.0;
4203 cartesian_trajectory_planner_B.b_I[4] = 1.0;
4204 cartesian_trajectory_planner_B.b_I[8] = 1.0;
4205 p = true;
4206 for (cartesian_trajectory_planner_B.b_k_p = 0;
4207 cartesian_trajectory_planner_B.b_k_p < 9;
4208 cartesian_trajectory_planner_B.b_k_p++) {
4209 cartesian_trajectory_planner_B.a_cm =
4210 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_p]
4211 - R[cartesian_trajectory_planner_B.b_k_p];
4212 if (p && ((!rtIsInf(cartesian_trajectory_planner_B.a_cm)) && (!rtIsNaN
4213 (cartesian_trajectory_planner_B.a_cm)))) {
4214 } else {
4215 p = false;
4216 }
4217
4218 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_p]
4219 = cartesian_trajectory_planner_B.a_cm;
4220 }
4221
4222 if (p) {
4223 cartesian_trajectory_planne_svd(cartesian_trajectory_planner_B.b_I,
4224 cartesian_trajectory_planner_B.b_U,
4225 cartesian_trajectory_planner_B.vspecial_data,
4226 cartesian_trajectory_planner_B.V_l);
4227 } else {
4228 for (cartesian_trajectory_planner_B.b_k_p = 0;
4229 cartesian_trajectory_planner_B.b_k_p < 9;
4230 cartesian_trajectory_planner_B.b_k_p++) {
4231 cartesian_trajectory_planner_B.V_l[cartesian_trajectory_planner_B.b_k_p]
4232 = (rtNaN);
4233 }
4234 }
4235
4236 cartesian_trajectory_planner_B.vspecial_data[0] =
4237 cartesian_trajectory_planner_B.V_l[6];
4238 cartesian_trajectory_planner_B.vspecial_data[1] =
4239 cartesian_trajectory_planner_B.V_l[7];
4240 cartesian_trajectory_planner_B.vspecial_data[2] =
4241 cartesian_trajectory_planner_B.V_l[8];
4242 }
4243
4244 loop_ub_tmp = 0;
4245 if (rEQ0 || e) {
4246 for (cartesian_trajectory_planner_B.loop_ub_nk = 0;
4247 cartesian_trajectory_planner_B.loop_ub_nk < 1;
4248 cartesian_trajectory_planner_B.loop_ub_nk++) {
4249 loop_ub_tmp++;
4250 }
4251 }
4252
4253 for (cartesian_trajectory_planner_B.b_k_p = 0;
4254 cartesian_trajectory_planner_B.b_k_p < loop_ub_tmp;
4255 cartesian_trajectory_planner_B.b_k_p++) {
4256 cartesian_trajectory_planner_B.v_nr[0] =
4257 cartesian_trajectory_planner_B.vspecial_data[3 *
4258 cartesian_trajectory_planner_B.b_k_p];
4259 cartesian_trajectory_planner_B.v_nr[1] =
4260 cartesian_trajectory_planner_B.vspecial_data[3 *
4261 cartesian_trajectory_planner_B.b_k_p + 1];
4262 cartesian_trajectory_planner_B.v_nr[2] =
4263 cartesian_trajectory_planner_B.vspecial_data[3 *
4264 cartesian_trajectory_planner_B.b_k_p + 2];
4265 }
4266 }
4267
4268 cartesian_trajectory_planner_B.a_cm = 1.0 / sqrt
4269 ((cartesian_trajectory_planner_B.v_nr[0] *
4270 cartesian_trajectory_planner_B.v_nr[0] +
4271 cartesian_trajectory_planner_B.v_nr[1] *
4272 cartesian_trajectory_planner_B.v_nr[1]) +
4273 cartesian_trajectory_planner_B.v_nr[2] *
4274 cartesian_trajectory_planner_B.v_nr[2]);
4275 cartesian_trajectory_planner_B.v_nr[0] *= cartesian_trajectory_planner_B.a_cm;
4276 cartesian_trajectory_planner_B.v_nr[1] *= cartesian_trajectory_planner_B.a_cm;
4277 axang[0] = cartesian_trajectory_planner_B.v_nr[0];
4278 axang[1] = cartesian_trajectory_planner_B.v_nr[1];
4279 axang[2] = cartesian_trajectory_planner_B.v_nr[2] *
4280 cartesian_trajectory_planner_B.a_cm;
4281 axang[3] = cartesian_trajectory_planner_B.v_fx.re;
4282}
4283
4284static void cartesian_IKHelpers_computeCost(const real_T x[6],
4285 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
4286 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args)
4287{
4288 x_robotics_manip_internal_Rig_T *treeInternal;
4289 emxArray_char_T_cartesian_tra_T *bodyName;
4290 emxArray_real_T_cartesian_tra_T *J;
4291 emxArray_real_T_cartesian_tra_T *y;
4292 cartesian_trajec_emxInit_char_T(&bodyName, 2);
4293 *b_args = args;
4294 treeInternal = args->Robot;
4295 cartesian_trajectory_planner_B.b_j_j = bodyName->size[0] * bodyName->size[1];
4296 bodyName->size[0] = 1;
4297 bodyName->size[1] = args->BodyName->size[1];
4298 cartes_emxEnsureCapacity_char_T(bodyName, cartesian_trajectory_planner_B.b_j_j);
4299 cartesian_trajectory_planner_B.loop_ub_h = args->BodyName->size[0] *
4300 args->BodyName->size[1] - 1;
4301 for (cartesian_trajectory_planner_B.b_j_j = 0;
4302 cartesian_trajectory_planner_B.b_j_j <=
4303 cartesian_trajectory_planner_B.loop_ub_h;
4304 cartesian_trajectory_planner_B.b_j_j++) {
4305 bodyName->data[cartesian_trajectory_planner_B.b_j_j] = args->BodyName->
4306 data[cartesian_trajectory_planner_B.b_j_j];
4307 }
4308
4309 for (cartesian_trajectory_planner_B.b_j_j = 0;
4310 cartesian_trajectory_planner_B.b_j_j < 16;
4311 cartesian_trajectory_planner_B.b_j_j++) {
4312 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.b_j_j] =
4313 args->Tform[cartesian_trajectory_planner_B.b_j_j];
4314 }
4315
4316 for (cartesian_trajectory_planner_B.b_j_j = 0;
4317 cartesian_trajectory_planner_B.b_j_j < 36;
4318 cartesian_trajectory_planner_B.b_j_j++) {
4319 W[cartesian_trajectory_planner_B.b_j_j] = args->
4320 WeightMatrix[cartesian_trajectory_planner_B.b_j_j];
4321 }
4322
4323 cartesian_trajec_emxInit_real_T(&J, 2);
4324 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,
4325 cartesian_trajectory_planner_B.T_data, cartesian_trajectory_planner_B.T_size,
4326 J);
4327 cartesian_trajectory_planner_B.b_j_j = Jac->size[0] * Jac->size[1];
4328 Jac->size[0] = 6;
4329 Jac->size[1] = J->size[1];
4330 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_j_j);
4331 cartesian_trajectory_planner_B.loop_ub_h = J->size[0] * J->size[1] - 1;
4332 cartesian_trajec_emxFree_char_T(&bodyName);
4333 for (cartesian_trajectory_planner_B.b_j_j = 0;
4334 cartesian_trajectory_planner_B.b_j_j <=
4335 cartesian_trajectory_planner_B.loop_ub_h;
4336 cartesian_trajectory_planner_B.b_j_j++) {
4337 Jac->data[cartesian_trajectory_planner_B.b_j_j] = -J->
4338 data[cartesian_trajectory_planner_B.b_j_j];
4339 }
4340
4341 cartesian_trajec_emxFree_real_T(&J);
4342 for (cartesian_trajectory_planner_B.b_j_j = 0;
4343 cartesian_trajectory_planner_B.b_j_j < 3;
4344 cartesian_trajectory_planner_B.b_j_j++) {
4345 cartesian_trajectory_planner_B.T_i[3 * cartesian_trajectory_planner_B.b_j_j]
4346 =
4347 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.b_j_j];
4348 cartesian_trajectory_planner_B.n_m = 3 *
4349 cartesian_trajectory_planner_B.b_j_j + 1;
4350 cartesian_trajectory_planner_B.T_i[cartesian_trajectory_planner_B.n_m] =
4351 cartesian_trajectory_planner_B.T_data
4352 [((cartesian_trajectory_planner_B.b_j_j + 1) +
4353 cartesian_trajectory_planner_B.T_size[0]) - 1];
4354 cartesian_trajectory_planner_B.boffset_k = 3 *
4355 cartesian_trajectory_planner_B.b_j_j + 2;
4356 cartesian_trajectory_planner_B.T_i[cartesian_trajectory_planner_B.boffset_k]
4357 = cartesian_trajectory_planner_B.T_data
4358 [((cartesian_trajectory_planner_B.b_j_j + 1) +
4359 (cartesian_trajectory_planner_B.T_size[0] << 1)) - 1];
4360 for (cartesian_trajectory_planner_B.loop_ub_h = 0;
4361 cartesian_trajectory_planner_B.loop_ub_h < 3;
4362 cartesian_trajectory_planner_B.loop_ub_h++) {
4363 cartesian_trajectory_planner_B.Td_tmp =
4364 cartesian_trajectory_planner_B.loop_ub_h + 3 *
4365 cartesian_trajectory_planner_B.b_j_j;
4366 cartesian_trajectory_planner_B.Td_f[cartesian_trajectory_planner_B.Td_tmp]
4367 = 0.0;
4368 cartesian_trajectory_planner_B.Td_f[cartesian_trajectory_planner_B.Td_tmp]
4369 += cartesian_trajectory_planner_B.T_i[3 *
4370 cartesian_trajectory_planner_B.b_j_j] *
4371 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_h];
4372 cartesian_trajectory_planner_B.Td_f[cartesian_trajectory_planner_B.Td_tmp]
4373 += cartesian_trajectory_planner_B.T_i[cartesian_trajectory_planner_B.n_m]
4374 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_h
4375 + 4];
4376 cartesian_trajectory_planner_B.Td_f[cartesian_trajectory_planner_B.Td_tmp]
4377 +=
4378 cartesian_trajectory_planner_B.T_i[cartesian_trajectory_planner_B.boffset_k]
4379 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_h
4380 + 8];
4381 }
4382 }
4383
4384 cartesian_trajectory_rotm2axang(cartesian_trajectory_planner_B.Td_f,
4385 cartesian_trajectory_planner_B.v);
4386 cartesian_trajectory_planner_B.e[0] = cartesian_trajectory_planner_B.v[3] *
4387 cartesian_trajectory_planner_B.v[0];
4388 cartesian_trajectory_planner_B.e[3] = cartesian_trajectory_planner_B.Td[12] -
4389 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4390 [0] * 3];
4391 cartesian_trajectory_planner_B.e[1] = cartesian_trajectory_planner_B.v[3] *
4392 cartesian_trajectory_planner_B.v[1];
4393 cartesian_trajectory_planner_B.e[4] = cartesian_trajectory_planner_B.Td[13] -
4394 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4395 [0] * 3 + 1];
4396 cartesian_trajectory_planner_B.e[2] = cartesian_trajectory_planner_B.v[3] *
4397 cartesian_trajectory_planner_B.v[2];
4398 cartesian_trajectory_planner_B.e[5] = cartesian_trajectory_planner_B.Td[14] -
4399 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4400 [0] * 3 + 2];
4401 cartesian_trajectory_planner_B.b_j_j = args->ErrTemp->size[0];
4402 args->ErrTemp->size[0] = 6;
4403 cartes_emxEnsureCapacity_real_T(args->ErrTemp,
4404 cartesian_trajectory_planner_B.b_j_j);
4405 for (cartesian_trajectory_planner_B.b_j_j = 0;
4406 cartesian_trajectory_planner_B.b_j_j < 6;
4407 cartesian_trajectory_planner_B.b_j_j++) {
4408 args->ErrTemp->data[cartesian_trajectory_planner_B.b_j_j] =
4409 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_j];
4410 }
4411
4412 for (cartesian_trajectory_planner_B.b_j_j = 0;
4413 cartesian_trajectory_planner_B.b_j_j < 6;
4414 cartesian_trajectory_planner_B.b_j_j++) {
4415 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j] = 0.0;
4416 for (cartesian_trajectory_planner_B.loop_ub_h = 0;
4417 cartesian_trajectory_planner_B.loop_ub_h < 6;
4418 cartesian_trajectory_planner_B.loop_ub_h++) {
4419 cartesian_trajectory_planner_B.s_g = W[6 *
4420 cartesian_trajectory_planner_B.b_j_j +
4421 cartesian_trajectory_planner_B.loop_ub_h] * (0.5 *
4422 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_h])
4423 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j];
4424 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j] =
4425 cartesian_trajectory_planner_B.s_g;
4426 }
4427 }
4428
4429 cartesian_trajectory_planner_B.s_g = 0.0;
4430 for (cartesian_trajectory_planner_B.b_j_j = 0;
4431 cartesian_trajectory_planner_B.b_j_j < 6;
4432 cartesian_trajectory_planner_B.b_j_j++) {
4433 cartesian_trajectory_planner_B.s_g +=
4434 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j] *
4435 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_j];
4436 }
4437
4438 args->CostTemp = cartesian_trajectory_planner_B.s_g;
4439 for (cartesian_trajectory_planner_B.b_j_j = 0;
4440 cartesian_trajectory_planner_B.b_j_j < 6;
4441 cartesian_trajectory_planner_B.b_j_j++) {
4442 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j] = 0.0;
4443 for (cartesian_trajectory_planner_B.loop_ub_h = 0;
4444 cartesian_trajectory_planner_B.loop_ub_h < 6;
4445 cartesian_trajectory_planner_B.loop_ub_h++) {
4446 cartesian_trajectory_planner_B.s_g = W[6 *
4447 cartesian_trajectory_planner_B.b_j_j +
4448 cartesian_trajectory_planner_B.loop_ub_h] *
4449 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_h]
4450 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j];
4451 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_j] =
4452 cartesian_trajectory_planner_B.s_g;
4453 }
4454 }
4455
4456 cartesian_trajec_emxInit_real_T(&y, 2);
4457 cartesian_trajectory_planner_B.n_m = Jac->size[1] - 1;
4458 cartesian_trajectory_planner_B.b_j_j = y->size[0] * y->size[1];
4459 y->size[0] = 1;
4460 y->size[1] = Jac->size[1];
4461 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_j_j);
4462 for (cartesian_trajectory_planner_B.b_j_j = 0;
4463 cartesian_trajectory_planner_B.b_j_j <=
4464 cartesian_trajectory_planner_B.n_m; cartesian_trajectory_planner_B.b_j_j
4465 ++) {
4466 cartesian_trajectory_planner_B.boffset_k =
4467 cartesian_trajectory_planner_B.b_j_j * 6 - 1;
4468 cartesian_trajectory_planner_B.s_g = 0.0;
4469 for (cartesian_trajectory_planner_B.loop_ub_h = 0;
4470 cartesian_trajectory_planner_B.loop_ub_h < 6;
4471 cartesian_trajectory_planner_B.loop_ub_h++) {
4472 cartesian_trajectory_planner_B.s_g += Jac->data
4473 [(cartesian_trajectory_planner_B.boffset_k +
4474 cartesian_trajectory_planner_B.loop_ub_h) + 1] *
4475 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.loop_ub_h];
4476 }
4477
4478 y->data[cartesian_trajectory_planner_B.b_j_j] =
4479 cartesian_trajectory_planner_B.s_g;
4480 }
4481
4482 cartesian_trajectory_planner_B.b_j_j = args->GradTemp->size[0];
4483 args->GradTemp->size[0] = y->size[1];
4484 cartes_emxEnsureCapacity_real_T(args->GradTemp,
4485 cartesian_trajectory_planner_B.b_j_j);
4486 cartesian_trajectory_planner_B.loop_ub_h = y->size[1];
4487 for (cartesian_trajectory_planner_B.b_j_j = 0;
4488 cartesian_trajectory_planner_B.b_j_j <
4489 cartesian_trajectory_planner_B.loop_ub_h;
4490 cartesian_trajectory_planner_B.b_j_j++) {
4491 args->GradTemp->data[cartesian_trajectory_planner_B.b_j_j] = y->
4492 data[cartesian_trajectory_planner_B.b_j_j];
4493 }
4494
4495 cartesian_trajec_emxFree_real_T(&y);
4496 cartesian_trajectory_planner_B.s_g = args->CostTemp;
4497 *cost = cartesian_trajectory_planner_B.s_g;
4498}
4499
4500static void cartesian_trajectory_planne_eye(real_T b_I[36])
4501{
4502 int32_T b_k;
4503 memset(&b_I[0], 0, 36U * sizeof(real_T));
4504 for (b_k = 0; b_k < 6; b_k++) {
4505 b_I[b_k + 6 * b_k] = 1.0;
4506 }
4507}
4508
4509static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
4510 **pEmxArray, int32_T numDimensions)
4511{
4512 emxArray_boolean_T_cartesian__T *emxArray;
4513 int32_T i;
4514 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
4515 (emxArray_boolean_T_cartesian__T));
4516 emxArray = *pEmxArray;
4517 emxArray->data = (boolean_T *)NULL;
4518 emxArray->numDimensions = numDimensions;
4519 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4520 emxArray->allocatedSize = 0;
4521 emxArray->canFreeData = true;
4522 for (i = 0; i < numDimensions; i++) {
4523 emxArray->size[i] = 0;
4524 }
4525}
4526
4527static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
4528 **pEmxArray, int32_T numDimensions)
4529{
4530 emxArray_int32_T_cartesian_tr_T *emxArray;
4531 int32_T i;
4532 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)malloc(sizeof
4533 (emxArray_int32_T_cartesian_tr_T));
4534 emxArray = *pEmxArray;
4535 emxArray->data = (int32_T *)NULL;
4536 emxArray->numDimensions = numDimensions;
4537 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4538 emxArray->allocatedSize = 0;
4539 emxArray->canFreeData = true;
4540 for (i = 0; i < numDimensions; i++) {
4541 emxArray->size[i] = 0;
4542 }
4543}
4544
4545static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
4546 *emxArray, int32_T oldNumel)
4547{
4548 int32_T newNumel;
4549 int32_T i;
4550 void *newData;
4551 if (oldNumel < 0) {
4552 oldNumel = 0;
4553 }
4554
4555 newNumel = 1;
4556 for (i = 0; i < emxArray->numDimensions; i++) {
4557 newNumel *= emxArray->size[i];
4558 }
4559
4560 if (newNumel > emxArray->allocatedSize) {
4561 i = emxArray->allocatedSize;
4562 if (i < 16) {
4563 i = 16;
4564 }
4565
4566 while (i < newNumel) {
4567 if (i > 1073741823) {
4568 i = MAX_int32_T;
4569 } else {
4570 i <<= 1;
4571 }
4572 }
4573
4574 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
4575 if (emxArray->data != NULL) {
4576 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
4577 if (emxArray->canFreeData) {
4578 free(emxArray->data);
4579 }
4580 }
4581
4582 emxArray->data = (boolean_T *)newData;
4583 emxArray->allocatedSize = i;
4584 emxArray->canFreeData = true;
4585 }
4586}
4587
4588static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
4589 *emxArray, int32_T oldNumel)
4590{
4591 int32_T newNumel;
4592 int32_T i;
4593 void *newData;
4594 if (oldNumel < 0) {
4595 oldNumel = 0;
4596 }
4597
4598 newNumel = 1;
4599 for (i = 0; i < emxArray->numDimensions; i++) {
4600 newNumel *= emxArray->size[i];
4601 }
4602
4603 if (newNumel > emxArray->allocatedSize) {
4604 i = emxArray->allocatedSize;
4605 if (i < 16) {
4606 i = 16;
4607 }
4608
4609 while (i < newNumel) {
4610 if (i > 1073741823) {
4611 i = MAX_int32_T;
4612 } else {
4613 i <<= 1;
4614 }
4615 }
4616
4617 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
4618 if (emxArray->data != NULL) {
4619 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
4620 if (emxArray->canFreeData) {
4621 free(emxArray->data);
4622 }
4623 }
4624
4625 emxArray->data = (int32_T *)newData;
4626 emxArray->allocatedSize = i;
4627 emxArray->canFreeData = true;
4628 }
4629}
4630
4631static real_T cartesian_trajectory_pla_norm_a(const real_T x[6])
4632{
4633 real_T y;
4634 real_T scale;
4635 real_T absxk;
4636 real_T t;
4637 int32_T b_k;
4638 y = 0.0;
4639 scale = 3.3121686421112381E-170;
4640 for (b_k = 0; b_k < 6; b_k++) {
4641 absxk = fabs(x[b_k]);
4642 if (absxk > scale) {
4643 t = scale / absxk;
4644 y = y * t * t + 1.0;
4645 scale = absxk;
4646 } else {
4647 t = absxk / scale;
4648 y += t * t;
4649 }
4650 }
4651
4652 return scale * sqrt(y);
4653}
4654
4655static real_T SystemTimeProvider_getElapsedTi(const
4656 f_robotics_core_internal_Syst_T *obj)
4657{
4658 real_T systemTime;
4659 systemTime = ctimefun();
4660 return systemTime - obj->StartTime;
4661}
4662
4663static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
4664 emxArray_real_T_cartesian_tra_T *x, int32_T ix0)
4665{
4666 real_T y;
4667 y = 0.0;
4668 if (n >= 1) {
4669 if (n == 1) {
4670 y = fabs(x->data[ix0 - 1]);
4671 } else {
4672 cartesian_trajectory_planner_B.scale_c = 3.3121686421112381E-170;
4673 cartesian_trajectory_planner_B.kend_e = ix0 + n;
4674 for (cartesian_trajectory_planner_B.k_in = ix0;
4675 cartesian_trajectory_planner_B.k_in <
4676 cartesian_trajectory_planner_B.kend_e;
4677 cartesian_trajectory_planner_B.k_in++) {
4678 cartesian_trajectory_planner_B.absxk_n = fabs(x->
4679 data[cartesian_trajectory_planner_B.k_in - 1]);
4680 if (cartesian_trajectory_planner_B.absxk_n >
4681 cartesian_trajectory_planner_B.scale_c) {
4682 cartesian_trajectory_planner_B.t_k =
4683 cartesian_trajectory_planner_B.scale_c /
4684 cartesian_trajectory_planner_B.absxk_n;
4685 y = y * cartesian_trajectory_planner_B.t_k *
4686 cartesian_trajectory_planner_B.t_k + 1.0;
4687 cartesian_trajectory_planner_B.scale_c =
4688 cartesian_trajectory_planner_B.absxk_n;
4689 } else {
4690 cartesian_trajectory_planner_B.t_k =
4691 cartesian_trajectory_planner_B.absxk_n /
4692 cartesian_trajectory_planner_B.scale_c;
4693 y += cartesian_trajectory_planner_B.t_k *
4694 cartesian_trajectory_planner_B.t_k;
4695 }
4696 }
4697
4698 y = cartesian_trajectory_planner_B.scale_c * sqrt(y);
4699 }
4700 }
4701
4702 return y;
4703}
4704
4705static void cartesian_trajectory_pla_qrpf_a(const
4706 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
4707 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
4708 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
4709 *b_jpvt)
4710{
4711 emxArray_real_T_cartesian_tra_T *work;
4712 emxArray_real_T_cartesian_tra_T *vn1;
4713 emxArray_real_T_cartesian_tra_T *vn2;
4714 emxArray_real_T_cartesian_tra_T *c_x;
4715 int32_T exitg1;
4716 boolean_T exitg2;
4717 cartesian_trajectory_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];
4718 b_jpvt->size[0] = 1;
4719 b_jpvt->size[1] = jpvt->size[1];
4720 carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_trajectory_planner_B.kend);
4721 cartesian_trajectory_planner_B.ix_m = jpvt->size[0] * jpvt->size[1] - 1;
4722 for (cartesian_trajectory_planner_B.kend = 0;
4723 cartesian_trajectory_planner_B.kend <=
4724 cartesian_trajectory_planner_B.ix_m; cartesian_trajectory_planner_B.kend
4725 ++) {
4726 b_jpvt->data[cartesian_trajectory_planner_B.kend] = jpvt->
4727 data[cartesian_trajectory_planner_B.kend];
4728 }
4729
4730 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
4731 b_A->size[0] = A->size[0];
4732 b_A->size[1] = A->size[1];
4733 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
4734 cartesian_trajectory_planner_B.ix_m = A->size[0] * A->size[1] - 1;
4735 for (cartesian_trajectory_planner_B.kend = 0;
4736 cartesian_trajectory_planner_B.kend <=
4737 cartesian_trajectory_planner_B.ix_m; cartesian_trajectory_planner_B.kend
4738 ++) {
4739 b_A->data[cartesian_trajectory_planner_B.kend] = A->
4740 data[cartesian_trajectory_planner_B.kend];
4741 }
4742
4743 cartesian_trajec_emxInit_real_T(&work, 1);
4744 cartesian_trajectory_planner_B.ma = A->size[0];
4745 if (m < n) {
4746 cartesian_trajectory_planner_B.m_g = m;
4747 } else {
4748 cartesian_trajectory_planner_B.m_g = n;
4749 }
4750
4751 cartesian_trajectory_planner_B.minmn_j = cartesian_trajectory_planner_B.m_g -
4752 1;
4753 cartesian_trajectory_planner_B.kend = work->size[0];
4754 work->size[0] = A->size[1];
4755 cartes_emxEnsureCapacity_real_T(work, cartesian_trajectory_planner_B.kend);
4756 cartesian_trajectory_planner_B.ix_m = A->size[1];
4757 for (cartesian_trajectory_planner_B.kend = 0;
4758 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_m;
4759 cartesian_trajectory_planner_B.kend++) {
4760 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
4761 }
4762
4763 cartesian_trajec_emxInit_real_T(&vn1, 1);
4764 cartesian_trajectory_planner_B.kend = vn1->size[0];
4765 vn1->size[0] = A->size[1];
4766 cartes_emxEnsureCapacity_real_T(vn1, cartesian_trajectory_planner_B.kend);
4767 cartesian_trajectory_planner_B.ix_m = A->size[1];
4768 for (cartesian_trajectory_planner_B.kend = 0;
4769 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_m;
4770 cartesian_trajectory_planner_B.kend++) {
4771 vn1->data[cartesian_trajectory_planner_B.kend] = 0.0;
4772 }
4773
4774 cartesian_trajec_emxInit_real_T(&vn2, 1);
4775 cartesian_trajectory_planner_B.kend = vn2->size[0];
4776 vn2->size[0] = A->size[1];
4777 cartes_emxEnsureCapacity_real_T(vn2, cartesian_trajectory_planner_B.kend);
4778 cartesian_trajectory_planner_B.ix_m = A->size[1];
4779 for (cartesian_trajectory_planner_B.kend = 0;
4780 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_m;
4781 cartesian_trajectory_planner_B.kend++) {
4782 vn2->data[cartesian_trajectory_planner_B.kend] = 0.0;
4783 }
4784
4785 for (cartesian_trajectory_planner_B.m_g = 0;
4786 cartesian_trajectory_planner_B.m_g < n;
4787 cartesian_trajectory_planner_B.m_g++) {
4788 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.m_g *
4789 cartesian_trajectory_planner_B.ma;
4790 cartesian_trajectory_planner_B.smax = 0.0;
4791 if (m >= 1) {
4792 if (m == 1) {
4793 cartesian_trajectory_planner_B.smax = fabs(A->
4794 data[cartesian_trajectory_planner_B.pvt]);
4795 } else {
4796 cartesian_trajectory_planner_B.scale_h = 3.3121686421112381E-170;
4797 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.pvt
4798 + m;
4799 for (cartesian_trajectory_planner_B.itemp =
4800 cartesian_trajectory_planner_B.pvt + 1;
4801 cartesian_trajectory_planner_B.itemp <=
4802 cartesian_trajectory_planner_B.kend;
4803 cartesian_trajectory_planner_B.itemp++) {
4804 cartesian_trajectory_planner_B.absxk = fabs(A->
4805 data[cartesian_trajectory_planner_B.itemp - 1]);
4806 if (cartesian_trajectory_planner_B.absxk >
4807 cartesian_trajectory_planner_B.scale_h) {
4808 cartesian_trajectory_planner_B.t =
4809 cartesian_trajectory_planner_B.scale_h /
4810 cartesian_trajectory_planner_B.absxk;
4811 cartesian_trajectory_planner_B.smax =
4812 cartesian_trajectory_planner_B.smax *
4813 cartesian_trajectory_planner_B.t *
4814 cartesian_trajectory_planner_B.t + 1.0;
4815 cartesian_trajectory_planner_B.scale_h =
4816 cartesian_trajectory_planner_B.absxk;
4817 } else {
4818 cartesian_trajectory_planner_B.t =
4819 cartesian_trajectory_planner_B.absxk /
4820 cartesian_trajectory_planner_B.scale_h;
4821 cartesian_trajectory_planner_B.smax +=
4822 cartesian_trajectory_planner_B.t *
4823 cartesian_trajectory_planner_B.t;
4824 }
4825 }
4826
4827 cartesian_trajectory_planner_B.smax =
4828 cartesian_trajectory_planner_B.scale_h * sqrt
4829 (cartesian_trajectory_planner_B.smax);
4830 }
4831 }
4832
4833 vn1->data[cartesian_trajectory_planner_B.m_g] =
4834 cartesian_trajectory_planner_B.smax;
4835 vn2->data[cartesian_trajectory_planner_B.m_g] = vn1->
4836 data[cartesian_trajectory_planner_B.m_g];
4837 }
4838
4839 cartesian_trajec_emxInit_real_T(&c_x, 2);
4840 for (cartesian_trajectory_planner_B.m_g = 0;
4841 cartesian_trajectory_planner_B.m_g <=
4842 cartesian_trajectory_planner_B.minmn_j;
4843 cartesian_trajectory_planner_B.m_g++) {
4844 cartesian_trajectory_planner_B.iy_i = cartesian_trajectory_planner_B.m_g *
4845 cartesian_trajectory_planner_B.ma;
4846 cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.iy_i +
4847 cartesian_trajectory_planner_B.m_g;
4848 cartesian_trajectory_planner_B.nmi = n - cartesian_trajectory_planner_B.m_g;
4849 cartesian_trajectory_planner_B.mmi = (m - cartesian_trajectory_planner_B.m_g)
4850 - 1;
4851 if (cartesian_trajectory_planner_B.nmi < 1) {
4852 cartesian_trajectory_planner_B.kend = 0;
4853 } else {
4854 cartesian_trajectory_planner_B.kend = 1;
4855 if (cartesian_trajectory_planner_B.nmi > 1) {
4856 cartesian_trajectory_planner_B.ix_m = cartesian_trajectory_planner_B.m_g;
4857 cartesian_trajectory_planner_B.smax = fabs(vn1->
4858 data[cartesian_trajectory_planner_B.m_g]);
4859 for (cartesian_trajectory_planner_B.itemp = 2;
4860 cartesian_trajectory_planner_B.itemp <=
4861 cartesian_trajectory_planner_B.nmi;
4862 cartesian_trajectory_planner_B.itemp++) {
4863 cartesian_trajectory_planner_B.ix_m++;
4864 cartesian_trajectory_planner_B.scale_h = fabs(vn1->
4865 data[cartesian_trajectory_planner_B.ix_m]);
4866 if (cartesian_trajectory_planner_B.scale_h >
4867 cartesian_trajectory_planner_B.smax) {
4868 cartesian_trajectory_planner_B.kend =
4869 cartesian_trajectory_planner_B.itemp;
4870 cartesian_trajectory_planner_B.smax =
4871 cartesian_trajectory_planner_B.scale_h;
4872 }
4873 }
4874 }
4875 }
4876
4877 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.m_g +
4878 cartesian_trajectory_planner_B.kend) - 1;
4879 if (cartesian_trajectory_planner_B.pvt + 1 !=
4880 cartesian_trajectory_planner_B.m_g + 1) {
4881 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
4882 c_x->size[0] = b_A->size[0];
4883 c_x->size[1] = b_A->size[1];
4884 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
4885 cartesian_trajectory_planner_B.ix_m = b_A->size[0] * b_A->size[1] - 1;
4886 for (cartesian_trajectory_planner_B.kend = 0;
4887 cartesian_trajectory_planner_B.kend <=
4888 cartesian_trajectory_planner_B.ix_m;
4889 cartesian_trajectory_planner_B.kend++) {
4890 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
4891 data[cartesian_trajectory_planner_B.kend];
4892 }
4893
4894 cartesian_trajectory_planner_B.ix_m = cartesian_trajectory_planner_B.pvt *
4895 cartesian_trajectory_planner_B.ma;
4896 for (cartesian_trajectory_planner_B.itemp = 0;
4897 cartesian_trajectory_planner_B.itemp < m;
4898 cartesian_trajectory_planner_B.itemp++) {
4899 cartesian_trajectory_planner_B.scale_h = c_x->
4900 data[cartesian_trajectory_planner_B.ix_m];
4901 c_x->data[cartesian_trajectory_planner_B.ix_m] = c_x->
4902 data[cartesian_trajectory_planner_B.iy_i];
4903 c_x->data[cartesian_trajectory_planner_B.iy_i] =
4904 cartesian_trajectory_planner_B.scale_h;
4905 cartesian_trajectory_planner_B.ix_m++;
4906 cartesian_trajectory_planner_B.iy_i++;
4907 }
4908
4909 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
4910 b_A->size[0] = c_x->size[0];
4911 b_A->size[1] = c_x->size[1];
4912 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
4913 cartesian_trajectory_planner_B.ix_m = c_x->size[0] * c_x->size[1] - 1;
4914 for (cartesian_trajectory_planner_B.kend = 0;
4915 cartesian_trajectory_planner_B.kend <=
4916 cartesian_trajectory_planner_B.ix_m;
4917 cartesian_trajectory_planner_B.kend++) {
4918 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
4919 data[cartesian_trajectory_planner_B.kend];
4920 }
4921
4922 cartesian_trajectory_planner_B.itemp = b_jpvt->
4923 data[cartesian_trajectory_planner_B.pvt];
4924 b_jpvt->data[cartesian_trajectory_planner_B.pvt] = b_jpvt->
4925 data[cartesian_trajectory_planner_B.m_g];
4926 b_jpvt->data[cartesian_trajectory_planner_B.m_g] =
4927 cartesian_trajectory_planner_B.itemp;
4928 vn1->data[cartesian_trajectory_planner_B.pvt] = vn1->
4929 data[cartesian_trajectory_planner_B.m_g];
4930 vn2->data[cartesian_trajectory_planner_B.pvt] = vn2->
4931 data[cartesian_trajectory_planner_B.m_g];
4932 }
4933
4934 if (cartesian_trajectory_planner_B.m_g + 1 < m) {
4935 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.ii + 2;
4936 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
4937 c_x->size[0] = b_A->size[0];
4938 c_x->size[1] = b_A->size[1];
4939 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
4940 cartesian_trajectory_planner_B.ix_m = b_A->size[0] * b_A->size[1] - 1;
4941 for (cartesian_trajectory_planner_B.kend = 0;
4942 cartesian_trajectory_planner_B.kend <=
4943 cartesian_trajectory_planner_B.ix_m;
4944 cartesian_trajectory_planner_B.kend++) {
4945 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
4946 data[cartesian_trajectory_planner_B.kend];
4947 }
4948
4949 cartesian_trajectory_planner_B.smax = b_A->
4950 data[cartesian_trajectory_planner_B.ii];
4951 tau->data[cartesian_trajectory_planner_B.m_g] = 0.0;
4952 if (cartesian_trajectory_planner_B.mmi + 1 > 0) {
4953 cartesian_trajectory_planner_B.scale_h = cartesian_trajectory_p_xnrm2_as
4954 (cartesian_trajectory_planner_B.mmi, b_A,
4955 cartesian_trajectory_planner_B.ii + 2);
4956 if (cartesian_trajectory_planner_B.scale_h != 0.0) {
4957 cartesian_trajectory_planner_B.scale_h = rt_hypotd_snf(b_A->
4958 data[cartesian_trajectory_planner_B.ii],
4959 cartesian_trajectory_planner_B.scale_h);
4960 if (b_A->data[cartesian_trajectory_planner_B.ii] >= 0.0) {
4961 cartesian_trajectory_planner_B.scale_h =
4962 -cartesian_trajectory_planner_B.scale_h;
4963 }
4964
4965 if (fabs(cartesian_trajectory_planner_B.scale_h) <
4966 1.0020841800044864E-292) {
4967 cartesian_trajectory_planner_B.kend = -1;
4968 cartesian_trajectory_planner_B.ix_m =
4969 (cartesian_trajectory_planner_B.ii +
4970 cartesian_trajectory_planner_B.mmi) + 1;
4971 do {
4972 cartesian_trajectory_planner_B.kend++;
4973 for (cartesian_trajectory_planner_B.itemp =
4974 cartesian_trajectory_planner_B.pvt;
4975 cartesian_trajectory_planner_B.itemp <=
4976 cartesian_trajectory_planner_B.ix_m;
4977 cartesian_trajectory_planner_B.itemp++) {
4978 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
4979 9.9792015476736E+291;
4980 }
4981
4982 cartesian_trajectory_planner_B.scale_h *= 9.9792015476736E+291;
4983 cartesian_trajectory_planner_B.smax *= 9.9792015476736E+291;
4984 } while (!(fabs(cartesian_trajectory_planner_B.scale_h) >=
4985 1.0020841800044864E-292));
4986
4987 cartesian_trajectory_planner_B.scale_h = rt_hypotd_snf
4988 (cartesian_trajectory_planner_B.smax,
4989 cartesian_trajectory_p_xnrm2_as
4990 (cartesian_trajectory_planner_B.mmi, c_x,
4991 cartesian_trajectory_planner_B.ii + 2));
4992 if (cartesian_trajectory_planner_B.smax >= 0.0) {
4993 cartesian_trajectory_planner_B.scale_h =
4994 -cartesian_trajectory_planner_B.scale_h;
4995 }
4996
4997 tau->data[cartesian_trajectory_planner_B.m_g] =
4998 (cartesian_trajectory_planner_B.scale_h -
4999 cartesian_trajectory_planner_B.smax) /
5000 cartesian_trajectory_planner_B.scale_h;
5001 cartesian_trajectory_planner_B.smax = 1.0 /
5002 (cartesian_trajectory_planner_B.smax -
5003 cartesian_trajectory_planner_B.scale_h);
5004 for (cartesian_trajectory_planner_B.itemp =
5005 cartesian_trajectory_planner_B.pvt;
5006 cartesian_trajectory_planner_B.itemp <=
5007 cartesian_trajectory_planner_B.ix_m;
5008 cartesian_trajectory_planner_B.itemp++) {
5009 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
5010 cartesian_trajectory_planner_B.smax;
5011 }
5012
5013 for (cartesian_trajectory_planner_B.itemp = 0;
5014 cartesian_trajectory_planner_B.itemp <=
5015 cartesian_trajectory_planner_B.kend;
5016 cartesian_trajectory_planner_B.itemp++) {
5017 cartesian_trajectory_planner_B.scale_h *= 1.0020841800044864E-292;
5018 }
5019
5020 cartesian_trajectory_planner_B.smax =
5021 cartesian_trajectory_planner_B.scale_h;
5022 } else {
5023 tau->data[cartesian_trajectory_planner_B.m_g] =
5024 (cartesian_trajectory_planner_B.scale_h - b_A->
5025 data[cartesian_trajectory_planner_B.ii]) /
5026 cartesian_trajectory_planner_B.scale_h;
5027 cartesian_trajectory_planner_B.smax = 1.0 / (b_A->
5028 data[cartesian_trajectory_planner_B.ii] -
5029 cartesian_trajectory_planner_B.scale_h);
5030 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5031 c_x->size[0] = b_A->size[0];
5032 c_x->size[1] = b_A->size[1];
5033 cartes_emxEnsureCapacity_real_T(c_x,
5034 cartesian_trajectory_planner_B.kend);
5035 cartesian_trajectory_planner_B.ix_m = b_A->size[0] * b_A->size[1] -
5036 1;
5037 for (cartesian_trajectory_planner_B.kend = 0;
5038 cartesian_trajectory_planner_B.kend <=
5039 cartesian_trajectory_planner_B.ix_m;
5040 cartesian_trajectory_planner_B.kend++) {
5041 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5042 data[cartesian_trajectory_planner_B.kend];
5043 }
5044
5045 cartesian_trajectory_planner_B.b_oq =
5046 (cartesian_trajectory_planner_B.ii +
5047 cartesian_trajectory_planner_B.mmi) + 1;
5048 for (cartesian_trajectory_planner_B.itemp =
5049 cartesian_trajectory_planner_B.pvt;
5050 cartesian_trajectory_planner_B.itemp <=
5051 cartesian_trajectory_planner_B.b_oq;
5052 cartesian_trajectory_planner_B.itemp++) {
5053 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
5054 cartesian_trajectory_planner_B.smax;
5055 }
5056
5057 cartesian_trajectory_planner_B.smax =
5058 cartesian_trajectory_planner_B.scale_h;
5059 }
5060 }
5061 }
5062
5063 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5064 b_A->size[0] = c_x->size[0];
5065 b_A->size[1] = c_x->size[1];
5066 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5067 cartesian_trajectory_planner_B.ix_m = c_x->size[0] * c_x->size[1] - 1;
5068 for (cartesian_trajectory_planner_B.kend = 0;
5069 cartesian_trajectory_planner_B.kend <=
5070 cartesian_trajectory_planner_B.ix_m;
5071 cartesian_trajectory_planner_B.kend++) {
5072 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
5073 data[cartesian_trajectory_planner_B.kend];
5074 }
5075
5076 b_A->data[cartesian_trajectory_planner_B.ii] =
5077 cartesian_trajectory_planner_B.smax;
5078 } else {
5079 tau->data[cartesian_trajectory_planner_B.m_g] = 0.0;
5080 }
5081
5082 if (cartesian_trajectory_planner_B.m_g + 1 < n) {
5083 cartesian_trajectory_planner_B.smax = b_A->
5084 data[cartesian_trajectory_planner_B.ii];
5085 b_A->data[cartesian_trajectory_planner_B.ii] = 1.0;
5086 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.ii +
5087 cartesian_trajectory_planner_B.ma) + 1;
5088 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5089 c_x->size[0] = b_A->size[0];
5090 c_x->size[1] = b_A->size[1];
5091 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5092 cartesian_trajectory_planner_B.ix_m = b_A->size[0] * b_A->size[1] - 1;
5093 for (cartesian_trajectory_planner_B.kend = 0;
5094 cartesian_trajectory_planner_B.kend <=
5095 cartesian_trajectory_planner_B.ix_m;
5096 cartesian_trajectory_planner_B.kend++) {
5097 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5098 data[cartesian_trajectory_planner_B.kend];
5099 }
5100
5101 if (tau->data[cartesian_trajectory_planner_B.m_g] != 0.0) {
5102 cartesian_trajectory_planner_B.itemp =
5103 cartesian_trajectory_planner_B.mmi;
5104 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.ii
5105 + cartesian_trajectory_planner_B.mmi;
5106 while ((cartesian_trajectory_planner_B.itemp + 1 > 0) && (b_A->
5107 data[cartesian_trajectory_planner_B.kend] == 0.0)) {
5108 cartesian_trajectory_planner_B.itemp--;
5109 cartesian_trajectory_planner_B.kend--;
5110 }
5111
5112 cartesian_trajectory_planner_B.nmi--;
5113 exitg2 = false;
5114 while ((!exitg2) && (cartesian_trajectory_planner_B.nmi > 0)) {
5115 cartesian_trajectory_planner_B.ix_m =
5116 (cartesian_trajectory_planner_B.nmi - 1) *
5117 cartesian_trajectory_planner_B.ma +
5118 cartesian_trajectory_planner_B.pvt;
5119 cartesian_trajectory_planner_B.kend =
5120 cartesian_trajectory_planner_B.ix_m;
5121 do {
5122 exitg1 = 0;
5123 if (cartesian_trajectory_planner_B.kend <=
5124 cartesian_trajectory_planner_B.ix_m +
5125 cartesian_trajectory_planner_B.itemp) {
5126 if (b_A->data[cartesian_trajectory_planner_B.kend - 1] != 0.0) {
5127 exitg1 = 1;
5128 } else {
5129 cartesian_trajectory_planner_B.kend++;
5130 }
5131 } else {
5132 cartesian_trajectory_planner_B.nmi--;
5133 exitg1 = 2;
5134 }
5135 } while (exitg1 == 0);
5136
5137 if (exitg1 == 1) {
5138 exitg2 = true;
5139 }
5140 }
5141
5142 cartesian_trajectory_planner_B.lastc =
5143 cartesian_trajectory_planner_B.nmi - 1;
5144 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5145 c_x->size[0] = b_A->size[0];
5146 c_x->size[1] = b_A->size[1];
5147 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5148 cartesian_trajectory_planner_B.ix_m = b_A->size[0] * b_A->size[1] - 1;
5149 for (cartesian_trajectory_planner_B.kend = 0;
5150 cartesian_trajectory_planner_B.kend <=
5151 cartesian_trajectory_planner_B.ix_m;
5152 cartesian_trajectory_planner_B.kend++) {
5153 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5154 data[cartesian_trajectory_planner_B.kend];
5155 }
5156 } else {
5157 cartesian_trajectory_planner_B.itemp = -1;
5158 cartesian_trajectory_planner_B.lastc = -1;
5159 }
5160
5161 if (cartesian_trajectory_planner_B.itemp + 1 > 0) {
5162 if (cartesian_trajectory_planner_B.lastc + 1 != 0) {
5163 for (cartesian_trajectory_planner_B.kend = 0;
5164 cartesian_trajectory_planner_B.kend <=
5165 cartesian_trajectory_planner_B.lastc;
5166 cartesian_trajectory_planner_B.kend++) {
5167 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
5168 }
5169
5170 cartesian_trajectory_planner_B.iy_i = 0;
5171 cartesian_trajectory_planner_B.b_oq =
5172 cartesian_trajectory_planner_B.ma *
5173 cartesian_trajectory_planner_B.lastc +
5174 cartesian_trajectory_planner_B.pvt;
5175 for (cartesian_trajectory_planner_B.nmi =
5176 cartesian_trajectory_planner_B.pvt;
5177 cartesian_trajectory_planner_B.ma < 0 ?
5178 cartesian_trajectory_planner_B.nmi >=
5179 cartesian_trajectory_planner_B.b_oq :
5180 cartesian_trajectory_planner_B.nmi <=
5181 cartesian_trajectory_planner_B.b_oq;
5182 cartesian_trajectory_planner_B.nmi +=
5183 cartesian_trajectory_planner_B.ma) {
5184 cartesian_trajectory_planner_B.ix_m =
5185 cartesian_trajectory_planner_B.ii;
5186 cartesian_trajectory_planner_B.scale_h = 0.0;
5187 cartesian_trajectory_planner_B.d_b =
5188 cartesian_trajectory_planner_B.nmi +
5189 cartesian_trajectory_planner_B.itemp;
5190 for (cartesian_trajectory_planner_B.kend =
5191 cartesian_trajectory_planner_B.nmi;
5192 cartesian_trajectory_planner_B.kend <=
5193 cartesian_trajectory_planner_B.d_b;
5194 cartesian_trajectory_planner_B.kend++) {
5195 cartesian_trajectory_planner_B.scale_h += c_x->
5196 data[cartesian_trajectory_planner_B.kend - 1] * c_x->
5197 data[cartesian_trajectory_planner_B.ix_m];
5198 cartesian_trajectory_planner_B.ix_m++;
5199 }
5200
5201 work->data[cartesian_trajectory_planner_B.iy_i] +=
5202 cartesian_trajectory_planner_B.scale_h;
5203 cartesian_trajectory_planner_B.iy_i++;
5204 }
5205 }
5206
5207 if (!(-tau->data[cartesian_trajectory_planner_B.m_g] == 0.0)) {
5208 cartesian_trajectory_planner_B.iy_i = 0;
5209 for (cartesian_trajectory_planner_B.kend = 0;
5210 cartesian_trajectory_planner_B.kend <=
5211 cartesian_trajectory_planner_B.lastc;
5212 cartesian_trajectory_planner_B.kend++) {
5213 if (work->data[cartesian_trajectory_planner_B.iy_i] != 0.0) {
5214 cartesian_trajectory_planner_B.scale_h = work->
5215 data[cartesian_trajectory_planner_B.iy_i] * -tau->
5216 data[cartesian_trajectory_planner_B.m_g];
5217 cartesian_trajectory_planner_B.ix_m =
5218 cartesian_trajectory_planner_B.ii;
5219 cartesian_trajectory_planner_B.b_oq =
5220 cartesian_trajectory_planner_B.itemp +
5221 cartesian_trajectory_planner_B.pvt;
5222 for (cartesian_trajectory_planner_B.nmi =
5223 cartesian_trajectory_planner_B.pvt;
5224 cartesian_trajectory_planner_B.nmi <=
5225 cartesian_trajectory_planner_B.b_oq;
5226 cartesian_trajectory_planner_B.nmi++) {
5227 c_x->data[cartesian_trajectory_planner_B.nmi - 1] += c_x->
5228 data[cartesian_trajectory_planner_B.ix_m] *
5229 cartesian_trajectory_planner_B.scale_h;
5230 cartesian_trajectory_planner_B.ix_m++;
5231 }
5232 }
5233
5234 cartesian_trajectory_planner_B.iy_i++;
5235 cartesian_trajectory_planner_B.pvt +=
5236 cartesian_trajectory_planner_B.ma;
5237 }
5238 }
5239 }
5240
5241 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5242 b_A->size[0] = c_x->size[0];
5243 b_A->size[1] = c_x->size[1];
5244 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5245 cartesian_trajectory_planner_B.ix_m = c_x->size[0] * c_x->size[1] - 1;
5246 for (cartesian_trajectory_planner_B.kend = 0;
5247 cartesian_trajectory_planner_B.kend <=
5248 cartesian_trajectory_planner_B.ix_m;
5249 cartesian_trajectory_planner_B.kend++) {
5250 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
5251 data[cartesian_trajectory_planner_B.kend];
5252 }
5253
5254 b_A->data[cartesian_trajectory_planner_B.ii] =
5255 cartesian_trajectory_planner_B.smax;
5256 }
5257
5258 for (cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.m_g
5259 + 2; cartesian_trajectory_planner_B.ii <= n;
5260 cartesian_trajectory_planner_B.ii++) {
5261 cartesian_trajectory_planner_B.pvt = ((cartesian_trajectory_planner_B.ii -
5262 1) * cartesian_trajectory_planner_B.ma +
5263 cartesian_trajectory_planner_B.m_g) + 1;
5264 if (vn1->data[cartesian_trajectory_planner_B.ii - 1] != 0.0) {
5265 cartesian_trajectory_planner_B.smax = fabs(b_A->
5266 data[cartesian_trajectory_planner_B.pvt - 1]) / vn1->
5267 data[cartesian_trajectory_planner_B.ii - 1];
5268 cartesian_trajectory_planner_B.smax = 1.0 -
5269 cartesian_trajectory_planner_B.smax *
5270 cartesian_trajectory_planner_B.smax;
5271 if (cartesian_trajectory_planner_B.smax < 0.0) {
5272 cartesian_trajectory_planner_B.smax = 0.0;
5273 }
5274
5275 cartesian_trajectory_planner_B.scale_h = vn1->
5276 data[cartesian_trajectory_planner_B.ii - 1] / vn2->
5277 data[cartesian_trajectory_planner_B.ii - 1];
5278 cartesian_trajectory_planner_B.scale_h =
5279 cartesian_trajectory_planner_B.scale_h *
5280 cartesian_trajectory_planner_B.scale_h *
5281 cartesian_trajectory_planner_B.smax;
5282 if (cartesian_trajectory_planner_B.scale_h <= 1.4901161193847656E-8) {
5283 if (cartesian_trajectory_planner_B.m_g + 1 < m) {
5284 cartesian_trajectory_planner_B.smax = 0.0;
5285 if (cartesian_trajectory_planner_B.mmi >= 1) {
5286 if (cartesian_trajectory_planner_B.mmi == 1) {
5287 cartesian_trajectory_planner_B.smax = fabs(b_A->
5288 data[cartesian_trajectory_planner_B.pvt]);
5289 } else {
5290 cartesian_trajectory_planner_B.scale_h = 3.3121686421112381E-170;
5291 cartesian_trajectory_planner_B.kend =
5292 cartesian_trajectory_planner_B.pvt +
5293 cartesian_trajectory_planner_B.mmi;
5294 for (cartesian_trajectory_planner_B.itemp =
5295 cartesian_trajectory_planner_B.pvt + 1;
5296 cartesian_trajectory_planner_B.itemp <=
5297 cartesian_trajectory_planner_B.kend;
5298 cartesian_trajectory_planner_B.itemp++) {
5299 cartesian_trajectory_planner_B.absxk = fabs(b_A->
5300 data[cartesian_trajectory_planner_B.itemp - 1]);
5301 if (cartesian_trajectory_planner_B.absxk >
5302 cartesian_trajectory_planner_B.scale_h) {
5303 cartesian_trajectory_planner_B.t =
5304 cartesian_trajectory_planner_B.scale_h /
5305 cartesian_trajectory_planner_B.absxk;
5306 cartesian_trajectory_planner_B.smax =
5307 cartesian_trajectory_planner_B.smax *
5308 cartesian_trajectory_planner_B.t *
5309 cartesian_trajectory_planner_B.t + 1.0;
5310 cartesian_trajectory_planner_B.scale_h =
5311 cartesian_trajectory_planner_B.absxk;
5312 } else {
5313 cartesian_trajectory_planner_B.t =
5314 cartesian_trajectory_planner_B.absxk /
5315 cartesian_trajectory_planner_B.scale_h;
5316 cartesian_trajectory_planner_B.smax +=
5317 cartesian_trajectory_planner_B.t *
5318 cartesian_trajectory_planner_B.t;
5319 }
5320 }
5321
5322 cartesian_trajectory_planner_B.smax =
5323 cartesian_trajectory_planner_B.scale_h * sqrt
5324 (cartesian_trajectory_planner_B.smax);
5325 }
5326 }
5327
5328 vn1->data[cartesian_trajectory_planner_B.ii - 1] =
5329 cartesian_trajectory_planner_B.smax;
5330 vn2->data[cartesian_trajectory_planner_B.ii - 1] = vn1->
5331 data[cartesian_trajectory_planner_B.ii - 1];
5332 } else {
5333 vn1->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
5334 vn2->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
5335 }
5336 } else {
5337 vn1->data[cartesian_trajectory_planner_B.ii - 1] *= sqrt
5338 (cartesian_trajectory_planner_B.smax);
5339 }
5340 }
5341 }
5342 }
5343
5344 cartesian_trajec_emxFree_real_T(&c_x);
5345 cartesian_trajec_emxFree_real_T(&vn2);
5346 cartesian_trajec_emxFree_real_T(&vn1);
5347 cartesian_trajec_emxFree_real_T(&work);
5348}
5349
5350static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
5351 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
5352 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
5353 int32_T *info)
5354{
5355 int32_T mmj;
5356 int32_T b;
5357 int32_T c;
5358 emxArray_real_T_cartesian_tra_T *c_x;
5359 int32_T ix;
5360 real_T smax;
5361 real_T s;
5362 int32_T iy;
5363 int32_T n_0;
5364 int32_T yk;
5365 int32_T k;
5366 int32_T jA;
5367 int32_T jy;
5368 int32_T c_0;
5369 int32_T c_tmp;
5370 k = b_A->size[0] * b_A->size[1];
5371 b_A->size[0] = A->size[0];
5372 b_A->size[1] = A->size[1];
5373 cartes_emxEnsureCapacity_real_T(b_A, k);
5374 iy = A->size[0] * A->size[1] - 1;
5375 for (k = 0; k <= iy; k++) {
5376 b_A->data[k] = A->data[k];
5377 }
5378
5379 if (m < n) {
5380 n_0 = m;
5381 } else {
5382 n_0 = n;
5383 }
5384
5385 if (n_0 < 1) {
5386 n_0 = 0;
5387 }
5388
5389 k = ipiv->size[0] * ipiv->size[1];
5390 ipiv->size[0] = 1;
5391 ipiv->size[1] = n_0;
5392 carte_emxEnsureCapacity_int32_T(ipiv, k);
5393 if (n_0 > 0) {
5394 ipiv->data[0] = 1;
5395 yk = 1;
5396 for (k = 2; k <= n_0; k++) {
5397 yk++;
5398 ipiv->data[k - 1] = yk;
5399 }
5400 }
5401
5402 yk = 0;
5403 cartesian_trajec_emxInit_real_T(&c_x, 2);
5404 if ((m < 1) || (n < 1)) {
5405 } else {
5406 n_0 = m - 1;
5407 if (n_0 >= n) {
5408 n_0 = n;
5409 }
5410
5411 b = n_0 - 1;
5412 for (n_0 = 0; n_0 <= b; n_0++) {
5413 mmj = m - n_0;
5414 c_tmp = (lda + 1) * n_0;
5415 c = c_tmp + 2;
5416 if (mmj < 1) {
5417 iy = 0;
5418 } else {
5419 iy = 1;
5420 if (mmj > 1) {
5421 ix = c - 2;
5422 smax = fabs(b_A->data[c_tmp]);
5423 for (k = 2; k <= mmj; k++) {
5424 ix++;
5425 s = fabs(b_A->data[ix]);
5426 if (s > smax) {
5427 iy = k;
5428 smax = s;
5429 }
5430 }
5431 }
5432 }
5433
5434 if (b_A->data[(c + iy) - 3] != 0.0) {
5435 if (iy - 1 != 0) {
5436 k = n_0 + iy;
5437 ipiv->data[n_0] = k;
5438 ix = c_x->size[0] * c_x->size[1];
5439 c_x->size[0] = b_A->size[0];
5440 c_x->size[1] = b_A->size[1];
5441 cartes_emxEnsureCapacity_real_T(c_x, ix);
5442 iy = b_A->size[0] * b_A->size[1] - 1;
5443 for (ix = 0; ix <= iy; ix++) {
5444 c_x->data[ix] = b_A->data[ix];
5445 }
5446
5447 ix = n_0;
5448 iy = k - 1;
5449 for (k = 0; k < n; k++) {
5450 smax = c_x->data[ix];
5451 c_x->data[ix] = c_x->data[iy];
5452 c_x->data[iy] = smax;
5453 ix += lda;
5454 iy += lda;
5455 }
5456
5457 k = b_A->size[0] * b_A->size[1];
5458 b_A->size[0] = c_x->size[0];
5459 b_A->size[1] = c_x->size[1];
5460 cartes_emxEnsureCapacity_real_T(b_A, k);
5461 iy = c_x->size[0] * c_x->size[1] - 1;
5462 for (k = 0; k <= iy; k++) {
5463 b_A->data[k] = c_x->data[k];
5464 }
5465 }
5466
5467 iy = c + mmj;
5468 for (k = c; k <= iy - 2; k++) {
5469 b_A->data[k - 1] /= b_A->data[c_tmp];
5470 }
5471 } else {
5472 yk = n_0 + 1;
5473 }
5474
5475 iy = (n - n_0) - 2;
5476 jy = c_tmp + lda;
5477 jA = jy + 1;
5478 for (k = 0; k <= iy; k++) {
5479 smax = b_A->data[jy];
5480 if (b_A->data[jy] != 0.0) {
5481 ix = c - 1;
5482 c_0 = (mmj + jA) - 1;
5483 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
5484 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
5485 ix++;
5486 }
5487 }
5488
5489 jy += lda;
5490 jA += lda;
5491 }
5492 }
5493
5494 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
5495 != 0.0))) {
5496 yk = m;
5497 }
5498 }
5499
5500 cartesian_trajec_emxFree_real_T(&c_x);
5501 *info = yk;
5502}
5503
5504static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
5505 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
5506 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
5507 emxArray_real_T_cartesian_tra_T *b_B)
5508{
5509 int32_T jBcol;
5510 int32_T kAcol;
5511 int32_T i;
5512 int32_T k;
5513 int32_T b;
5514 int32_T b_i;
5515 int32_T loop_ub;
5516 int32_T i_0;
5517 int32_T tmp;
5518 i_0 = b_B->size[0] * b_B->size[1];
5519 b_B->size[0] = B->size[0];
5520 b_B->size[1] = B->size[1];
5521 cartes_emxEnsureCapacity_real_T(b_B, i_0);
5522 loop_ub = B->size[0] * B->size[1] - 1;
5523 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
5524 b_B->data[i_0] = B->data[i_0];
5525 }
5526
5527 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
5528 } else {
5529 for (loop_ub = 0; loop_ub < n; loop_ub++) {
5530 jBcol = ldb * loop_ub - 1;
5531 for (k = m; k >= 1; k--) {
5532 kAcol = (k - 1) * lda - 1;
5533 i_0 = k + jBcol;
5534 if (b_B->data[i_0] != 0.0) {
5535 b_B->data[i_0] /= A->data[k + kAcol];
5536 b = k - 2;
5537 for (b_i = 0; b_i <= b; b_i++) {
5538 i = b_i + 1;
5539 tmp = i + jBcol;
5540 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
5541 }
5542 }
5543 }
5544 }
5545 }
5546}
5547
5548static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
5549 **pEmxArray)
5550{
5551 if (*pEmxArray != (emxArray_int32_T_cartesian_tr_T *)NULL) {
5552 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
5553 free((*pEmxArray)->data);
5554 }
5555
5556 free((*pEmxArray)->size);
5557 free(*pEmxArray);
5558 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)NULL;
5559 }
5560}
5561
5562static void cartesian_trajectory_p_mldivide(const
5563 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
5564 emxArray_real_T_cartesian_tra_T *Y)
5565{
5566 emxArray_real_T_cartesian_tra_T *c_A;
5567 emxArray_real_T_cartesian_tra_T *b_tau;
5568 emxArray_int32_T_cartesian_tr_T *b_jpvt;
5569 emxArray_real_T_cartesian_tra_T *B_0;
5570 emxArray_int32_T_cartesian_tr_T *b_jpvt_0;
5571 boolean_T guard1 = false;
5572 cartesian_trajec_emxInit_real_T(&c_A, 2);
5573 cartesian_trajec_emxInit_real_T(&b_tau, 1);
5574 cartesian_traje_emxInit_int32_T(&b_jpvt, 2);
5575 cartesian_trajec_emxInit_real_T(&B_0, 2);
5576 cartesian_traje_emxInit_int32_T(&b_jpvt_0, 2);
5577 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
5578 == 0))) {
5579 cartesian_trajectory_planner_B.minmn = A->size[1];
5580 cartesian_trajectory_planner_B.minmana = B->size[1];
5581 cartesian_trajectory_planner_B.b_i_c = Y->size[0] * Y->size[1];
5582 Y->size[0] = cartesian_trajectory_planner_B.minmn;
5583 Y->size[1] = cartesian_trajectory_planner_B.minmana;
5584 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_c);
5585 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
5586 cartesian_trajectory_planner_B.minmana - 1;
5587 for (cartesian_trajectory_planner_B.b_i_c = 0;
5588 cartesian_trajectory_planner_B.b_i_c <=
5589 cartesian_trajectory_planner_B.minmn;
5590 cartesian_trajectory_planner_B.b_i_c++) {
5591 Y->data[cartesian_trajectory_planner_B.b_i_c] = 0.0;
5592 }
5593 } else if (A->size[0] == A->size[1]) {
5594 cartesian_trajectory_planner_B.minmn = A->size[0];
5595 cartesian_trajectory_planner_B.rankR = A->size[1];
5596 if (cartesian_trajectory_planner_B.minmn <
5597 cartesian_trajectory_planner_B.rankR) {
5598 cartesian_trajectory_planner_B.rankR =
5599 cartesian_trajectory_planner_B.minmn;
5600 }
5601
5602 cartesian_trajectory_planner_B.minmn = B->size[0];
5603 if (cartesian_trajectory_planner_B.minmn <
5604 cartesian_trajectory_planner_B.rankR) {
5605 cartesian_trajectory_planner_B.rankR =
5606 cartesian_trajectory_planner_B.minmn;
5607 }
5608
5609 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
5610 cartesian_trajectory_pl_xzgetrf(cartesian_trajectory_planner_B.rankR,
5611 cartesian_trajectory_planner_B.rankR, A, A->size[0], c_A, b_jpvt,
5612 &cartesian_trajectory_planner_B.minmn);
5613 cartesian_trajectory_planner_B.b_i_c = B_0->size[0] * B_0->size[1];
5614 B_0->size[0] = B->size[0];
5615 B_0->size[1] = B->size[1];
5616 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_c);
5617 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
5618 for (cartesian_trajectory_planner_B.b_i_c = 0;
5619 cartesian_trajectory_planner_B.b_i_c <=
5620 cartesian_trajectory_planner_B.minmn;
5621 cartesian_trajectory_planner_B.b_i_c++) {
5622 B_0->data[cartesian_trajectory_planner_B.b_i_c] = B->
5623 data[cartesian_trajectory_planner_B.b_i_c];
5624 }
5625
5626 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.rankR
5627 - 2;
5628 for (cartesian_trajectory_planner_B.b_i_c = 0;
5629 cartesian_trajectory_planner_B.b_i_c <=
5630 cartesian_trajectory_planner_B.minmn;
5631 cartesian_trajectory_planner_B.b_i_c++) {
5632 if (cartesian_trajectory_planner_B.b_i_c + 1 != b_jpvt->
5633 data[cartesian_trajectory_planner_B.b_i_c]) {
5634 cartesian_trajectory_planner_B.na = b_jpvt->
5635 data[cartesian_trajectory_planner_B.b_i_c] - 1;
5636 for (cartesian_trajectory_planner_B.minmana = 0;
5637 cartesian_trajectory_planner_B.minmana <=
5638 cartesian_trajectory_planner_B.nb;
5639 cartesian_trajectory_planner_B.minmana++) {
5640 cartesian_trajectory_planner_B.tol_j = B_0->data[B_0->size[0] *
5641 cartesian_trajectory_planner_B.minmana +
5642 cartesian_trajectory_planner_B.b_i_c];
5643 B_0->data[cartesian_trajectory_planner_B.b_i_c + B_0->size[0] *
5644 cartesian_trajectory_planner_B.minmana] = B_0->data[B_0->size[0] *
5645 cartesian_trajectory_planner_B.minmana +
5646 cartesian_trajectory_planner_B.na];
5647 B_0->data[cartesian_trajectory_planner_B.na + B_0->size[0] *
5648 cartesian_trajectory_planner_B.minmana] =
5649 cartesian_trajectory_planner_B.tol_j;
5650 }
5651 }
5652 }
5653
5654 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
5655 } else {
5656 for (cartesian_trajectory_planner_B.minmana = 0;
5657 cartesian_trajectory_planner_B.minmana <=
5658 cartesian_trajectory_planner_B.nb;
5659 cartesian_trajectory_planner_B.minmana++) {
5660 cartesian_trajectory_planner_B.m_d4 = B->size[0] *
5661 cartesian_trajectory_planner_B.minmana - 1;
5662 for (cartesian_trajectory_planner_B.minmn = 0;
5663 cartesian_trajectory_planner_B.minmn <
5664 cartesian_trajectory_planner_B.rankR;
5665 cartesian_trajectory_planner_B.minmn++) {
5666 cartesian_trajectory_planner_B.nb_k = c_A->size[0] *
5667 cartesian_trajectory_planner_B.minmn - 1;
5668 cartesian_trajectory_planner_B.b_i_c =
5669 (cartesian_trajectory_planner_B.minmn +
5670 cartesian_trajectory_planner_B.m_d4) + 1;
5671 if (B_0->data[cartesian_trajectory_planner_B.b_i_c] != 0.0) {
5672 for (cartesian_trajectory_planner_B.na =
5673 cartesian_trajectory_planner_B.minmn + 2;
5674 cartesian_trajectory_planner_B.na <=
5675 cartesian_trajectory_planner_B.rankR;
5676 cartesian_trajectory_planner_B.na++) {
5677 cartesian_trajectory_planner_B.mn =
5678 cartesian_trajectory_planner_B.na +
5679 cartesian_trajectory_planner_B.m_d4;
5680 B_0->data[cartesian_trajectory_planner_B.mn] -= B_0->
5681 data[cartesian_trajectory_planner_B.b_i_c] * c_A->
5682 data[cartesian_trajectory_planner_B.na +
5683 cartesian_trajectory_planner_B.nb_k];
5684 }
5685 }
5686 }
5687 }
5688 }
5689
5690 cartesian_trajectory_plan_xtrsm(cartesian_trajectory_planner_B.rankR,
5691 B->size[1], c_A, c_A->size[0], B_0, B->size[0], Y);
5692 } else {
5693 cartesian_trajectory_planner_B.na = A->size[1] - 1;
5694 cartesian_trajectory_planner_B.b_i_c = c_A->size[0] * c_A->size[1];
5695 c_A->size[0] = A->size[0];
5696 c_A->size[1] = A->size[1];
5697 cartes_emxEnsureCapacity_real_T(c_A, cartesian_trajectory_planner_B.b_i_c);
5698 cartesian_trajectory_planner_B.minmn = A->size[0] * A->size[1] - 1;
5699 for (cartesian_trajectory_planner_B.b_i_c = 0;
5700 cartesian_trajectory_planner_B.b_i_c <=
5701 cartesian_trajectory_planner_B.minmn;
5702 cartesian_trajectory_planner_B.b_i_c++) {
5703 c_A->data[cartesian_trajectory_planner_B.b_i_c] = A->
5704 data[cartesian_trajectory_planner_B.b_i_c];
5705 }
5706
5707 cartesian_trajectory_planner_B.minmn = A->size[0];
5708 cartesian_trajectory_planner_B.minmana = A->size[1];
5709 if (cartesian_trajectory_planner_B.minmn <
5710 cartesian_trajectory_planner_B.minmana) {
5711 cartesian_trajectory_planner_B.minmana =
5712 cartesian_trajectory_planner_B.minmn;
5713 }
5714
5715 cartesian_trajectory_planner_B.b_i_c = b_tau->size[0];
5716 b_tau->size[0] = cartesian_trajectory_planner_B.minmana;
5717 cartes_emxEnsureCapacity_real_T(b_tau, cartesian_trajectory_planner_B.b_i_c);
5718 for (cartesian_trajectory_planner_B.b_i_c = 0;
5719 cartesian_trajectory_planner_B.b_i_c <
5720 cartesian_trajectory_planner_B.minmana;
5721 cartesian_trajectory_planner_B.b_i_c++) {
5722 b_tau->data[cartesian_trajectory_planner_B.b_i_c] = 0.0;
5723 }
5724
5725 guard1 = false;
5726 if ((A->size[0] == 0) || (A->size[1] == 0)) {
5727 guard1 = true;
5728 } else {
5729 cartesian_trajectory_planner_B.minmn = A->size[0];
5730 cartesian_trajectory_planner_B.minmana = A->size[1];
5731 if (cartesian_trajectory_planner_B.minmn <
5732 cartesian_trajectory_planner_B.minmana) {
5733 cartesian_trajectory_planner_B.minmana =
5734 cartesian_trajectory_planner_B.minmn;
5735 }
5736
5737 if (cartesian_trajectory_planner_B.minmana < 1) {
5738 guard1 = true;
5739 } else {
5740 cartesian_trajectory_planner_B.b_i_c = b_jpvt->size[0] * b_jpvt->size[1];
5741 b_jpvt->size[0] = 1;
5742 b_jpvt->size[1] = A->size[1];
5743 carte_emxEnsureCapacity_int32_T(b_jpvt,
5744 cartesian_trajectory_planner_B.b_i_c);
5745 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
5746 for (cartesian_trajectory_planner_B.b_i_c = 0;
5747 cartesian_trajectory_planner_B.b_i_c <=
5748 cartesian_trajectory_planner_B.minmn;
5749 cartesian_trajectory_planner_B.b_i_c++) {
5750 b_jpvt->data[cartesian_trajectory_planner_B.b_i_c] = 0;
5751 }
5752
5753 for (cartesian_trajectory_planner_B.minmn = 0;
5754 cartesian_trajectory_planner_B.minmn <=
5755 cartesian_trajectory_planner_B.na;
5756 cartesian_trajectory_planner_B.minmn++) {
5757 b_jpvt->data[cartesian_trajectory_planner_B.minmn] =
5758 cartesian_trajectory_planner_B.minmn + 1;
5759 }
5760
5761 cartesian_trajectory_planner_B.b_i_c = b_jpvt_0->size[0] *
5762 b_jpvt_0->size[1];
5763 b_jpvt_0->size[0] = 1;
5764 b_jpvt_0->size[1] = b_jpvt->size[1];
5765 carte_emxEnsureCapacity_int32_T(b_jpvt_0,
5766 cartesian_trajectory_planner_B.b_i_c);
5767 cartesian_trajectory_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];
5768 for (cartesian_trajectory_planner_B.b_i_c = 0;
5769 cartesian_trajectory_planner_B.b_i_c <
5770 cartesian_trajectory_planner_B.minmn;
5771 cartesian_trajectory_planner_B.b_i_c++) {
5772 b_jpvt_0->data[cartesian_trajectory_planner_B.b_i_c] = b_jpvt->
5773 data[cartesian_trajectory_planner_B.b_i_c];
5774 }
5775
5776 cartesian_trajectory_pla_qrpf_a(A, A->size[0], A->size[1], b_tau,
5777 b_jpvt_0, c_A, b_jpvt);
5778 }
5779 }
5780
5781 if (guard1) {
5782 cartesian_trajectory_planner_B.b_i_c = b_jpvt->size[0] * b_jpvt->size[1];
5783 b_jpvt->size[0] = 1;
5784 b_jpvt->size[1] = A->size[1];
5785 carte_emxEnsureCapacity_int32_T(b_jpvt,
5786 cartesian_trajectory_planner_B.b_i_c);
5787 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
5788 for (cartesian_trajectory_planner_B.b_i_c = 0;
5789 cartesian_trajectory_planner_B.b_i_c <=
5790 cartesian_trajectory_planner_B.minmn;
5791 cartesian_trajectory_planner_B.b_i_c++) {
5792 b_jpvt->data[cartesian_trajectory_planner_B.b_i_c] = 0;
5793 }
5794
5795 for (cartesian_trajectory_planner_B.minmana = 0;
5796 cartesian_trajectory_planner_B.minmana <=
5797 cartesian_trajectory_planner_B.na;
5798 cartesian_trajectory_planner_B.minmana++) {
5799 b_jpvt->data[cartesian_trajectory_planner_B.minmana] =
5800 cartesian_trajectory_planner_B.minmana + 1;
5801 }
5802 }
5803
5804 cartesian_trajectory_planner_B.rankR = 0;
5805 if (c_A->size[0] < c_A->size[1]) {
5806 cartesian_trajectory_planner_B.minmn = c_A->size[0];
5807 cartesian_trajectory_planner_B.minmana = c_A->size[1];
5808 } else {
5809 cartesian_trajectory_planner_B.minmn = c_A->size[1];
5810 cartesian_trajectory_planner_B.minmana = c_A->size[0];
5811 }
5812
5813 if (cartesian_trajectory_planner_B.minmn > 0) {
5814 cartesian_trajectory_planner_B.tol_j = 2.2204460492503131E-15 *
5815 static_cast<real_T>(cartesian_trajectory_planner_B.minmana);
5816 if (1.4901161193847656E-8 < cartesian_trajectory_planner_B.tol_j) {
5817 cartesian_trajectory_planner_B.tol_j = 1.4901161193847656E-8;
5818 }
5819
5820 cartesian_trajectory_planner_B.tol_j *= fabs(c_A->data[0]);
5821 while ((cartesian_trajectory_planner_B.rankR <
5822 cartesian_trajectory_planner_B.minmn) && (!(fabs(c_A->data
5823 [c_A->size[0] * cartesian_trajectory_planner_B.rankR +
5824 cartesian_trajectory_planner_B.rankR]) <=
5825 cartesian_trajectory_planner_B.tol_j))) {
5826 cartesian_trajectory_planner_B.rankR++;
5827 }
5828 }
5829
5830 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
5831 cartesian_trajectory_planner_B.minmn = c_A->size[1];
5832 cartesian_trajectory_planner_B.minmana = B->size[1];
5833 cartesian_trajectory_planner_B.b_i_c = Y->size[0] * Y->size[1];
5834 Y->size[0] = cartesian_trajectory_planner_B.minmn;
5835 Y->size[1] = cartesian_trajectory_planner_B.minmana;
5836 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_c);
5837 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
5838 cartesian_trajectory_planner_B.minmana - 1;
5839 for (cartesian_trajectory_planner_B.b_i_c = 0;
5840 cartesian_trajectory_planner_B.b_i_c <=
5841 cartesian_trajectory_planner_B.minmn;
5842 cartesian_trajectory_planner_B.b_i_c++) {
5843 Y->data[cartesian_trajectory_planner_B.b_i_c] = 0.0;
5844 }
5845
5846 cartesian_trajectory_planner_B.b_i_c = B_0->size[0] * B_0->size[1];
5847 B_0->size[0] = B->size[0];
5848 B_0->size[1] = B->size[1];
5849 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_c);
5850 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
5851 for (cartesian_trajectory_planner_B.b_i_c = 0;
5852 cartesian_trajectory_planner_B.b_i_c <=
5853 cartesian_trajectory_planner_B.minmn;
5854 cartesian_trajectory_planner_B.b_i_c++) {
5855 B_0->data[cartesian_trajectory_planner_B.b_i_c] = B->
5856 data[cartesian_trajectory_planner_B.b_i_c];
5857 }
5858
5859 cartesian_trajectory_planner_B.m_d4 = c_A->size[0];
5860 cartesian_trajectory_planner_B.nb_k = B->size[1] - 1;
5861 cartesian_trajectory_planner_B.minmn = c_A->size[0];
5862 cartesian_trajectory_planner_B.minmana = c_A->size[1];
5863 if (cartesian_trajectory_planner_B.minmn <
5864 cartesian_trajectory_planner_B.minmana) {
5865 cartesian_trajectory_planner_B.minmana =
5866 cartesian_trajectory_planner_B.minmn;
5867 }
5868
5869 cartesian_trajectory_planner_B.mn = cartesian_trajectory_planner_B.minmana -
5870 1;
5871 for (cartesian_trajectory_planner_B.minmana = 0;
5872 cartesian_trajectory_planner_B.minmana <=
5873 cartesian_trajectory_planner_B.mn;
5874 cartesian_trajectory_planner_B.minmana++) {
5875 if (b_tau->data[cartesian_trajectory_planner_B.minmana] != 0.0) {
5876 for (cartesian_trajectory_planner_B.minmn = 0;
5877 cartesian_trajectory_planner_B.minmn <=
5878 cartesian_trajectory_planner_B.nb_k;
5879 cartesian_trajectory_planner_B.minmn++) {
5880 cartesian_trajectory_planner_B.tol_j = B_0->data[B_0->size[0] *
5881 cartesian_trajectory_planner_B.minmn +
5882 cartesian_trajectory_planner_B.minmana];
5883 for (cartesian_trajectory_planner_B.na =
5884 cartesian_trajectory_planner_B.minmana + 2;
5885 cartesian_trajectory_planner_B.na <=
5886 cartesian_trajectory_planner_B.m_d4;
5887 cartesian_trajectory_planner_B.na++) {
5888 cartesian_trajectory_planner_B.tol_j += c_A->data[(c_A->size[0] *
5889 cartesian_trajectory_planner_B.minmana +
5890 cartesian_trajectory_planner_B.na) - 1] * B_0->data[(B_0->size[0] *
5891 cartesian_trajectory_planner_B.minmn +
5892 cartesian_trajectory_planner_B.na) - 1];
5893 }
5894
5895 cartesian_trajectory_planner_B.tol_j *= b_tau->
5896 data[cartesian_trajectory_planner_B.minmana];
5897 if (cartesian_trajectory_planner_B.tol_j != 0.0) {
5898 B_0->data[cartesian_trajectory_planner_B.minmana + B_0->size[0] *
5899 cartesian_trajectory_planner_B.minmn] -=
5900 cartesian_trajectory_planner_B.tol_j;
5901 for (cartesian_trajectory_planner_B.b_i_c =
5902 cartesian_trajectory_planner_B.minmana + 2;
5903 cartesian_trajectory_planner_B.b_i_c <=
5904 cartesian_trajectory_planner_B.m_d4;
5905 cartesian_trajectory_planner_B.b_i_c++) {
5906 B_0->data[(cartesian_trajectory_planner_B.b_i_c + B_0->size[0] *
5907 cartesian_trajectory_planner_B.minmn) - 1] -= c_A->
5908 data[(c_A->size[0] * cartesian_trajectory_planner_B.minmana +
5909 cartesian_trajectory_planner_B.b_i_c) - 1] *
5910 cartesian_trajectory_planner_B.tol_j;
5911 }
5912 }
5913 }
5914 }
5915 }
5916
5917 for (cartesian_trajectory_planner_B.minmn = 0;
5918 cartesian_trajectory_planner_B.minmn <=
5919 cartesian_trajectory_planner_B.nb; cartesian_trajectory_planner_B.minmn
5920 ++) {
5921 for (cartesian_trajectory_planner_B.b_i_c = 0;
5922 cartesian_trajectory_planner_B.b_i_c <
5923 cartesian_trajectory_planner_B.rankR;
5924 cartesian_trajectory_planner_B.b_i_c++) {
5925 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_c] + Y->size[0]
5926 * cartesian_trajectory_planner_B.minmn) - 1] = B_0->data
5927 [B_0->size[0] * cartesian_trajectory_planner_B.minmn +
5928 cartesian_trajectory_planner_B.b_i_c];
5929 }
5930
5931 for (cartesian_trajectory_planner_B.minmana =
5932 cartesian_trajectory_planner_B.rankR;
5933 cartesian_trajectory_planner_B.minmana >= 1;
5934 cartesian_trajectory_planner_B.minmana--) {
5935 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] +
5936 Y->size[0] * cartesian_trajectory_planner_B.minmn) - 1] /=
5937 c_A->data[((cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0]
5938 + cartesian_trajectory_planner_B.minmana) - 1];
5939 cartesian_trajectory_planner_B.na =
5940 cartesian_trajectory_planner_B.minmana - 2;
5941 for (cartesian_trajectory_planner_B.b_i_c = 0;
5942 cartesian_trajectory_planner_B.b_i_c <=
5943 cartesian_trajectory_planner_B.na;
5944 cartesian_trajectory_planner_B.b_i_c++) {
5945 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_c] + Y->size
5946 [0] * cartesian_trajectory_planner_B.minmn) - 1] -= Y->data
5947 [(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] + Y->
5948 size[0] * cartesian_trajectory_planner_B.minmn) - 1] * c_A->data
5949 [(cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0] +
5950 cartesian_trajectory_planner_B.b_i_c];
5951 }
5952 }
5953 }
5954 }
5955
5956 cartesian_traje_emxFree_int32_T(&b_jpvt_0);
5957 cartesian_trajec_emxFree_real_T(&B_0);
5958 cartesian_traje_emxFree_int32_T(&b_jpvt);
5959 cartesian_trajec_emxFree_real_T(&b_tau);
5960 cartesian_trajec_emxFree_real_T(&c_A);
5961}
5962
5963static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
5964 **pEmxArray)
5965{
5966 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
5967 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
5968 {
5969 free((*pEmxArray)->data);
5970 }
5971
5972 free((*pEmxArray)->size);
5973 free(*pEmxArray);
5974 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
5975 }
5976}
5977
5978static boolean_T DampedBFGSwGradientProjection_a(const
5979 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
5980 emxArray_real_T_cartesian_tra_T *alpha)
5981{
5982 boolean_T flag;
5983 boolean_T y;
5984 emxArray_boolean_T_cartesian__T *x;
5985 int32_T ix;
5986 int32_T loop_ub;
5987 boolean_T exitg1;
5988 cartesian_tra_emxInit_boolean_T(&x, 1);
5989 if (cartesian_trajectory_pla_norm_a(Hg) < obj->GradientTolerance) {
5990 ix = x->size[0];
5991 x->size[0] = alpha->size[0];
5992 car_emxEnsureCapacity_boolean_T(x, ix);
5993 loop_ub = alpha->size[0];
5994 for (ix = 0; ix < loop_ub; ix++) {
5995 x->data[ix] = (alpha->data[ix] <= 0.0);
5996 }
5997
5998 y = true;
5999 ix = 0;
6000 exitg1 = false;
6001 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6002 if (!x->data[ix]) {
6003 y = false;
6004 exitg1 = true;
6005 } else {
6006 ix++;
6007 }
6008 }
6009
6010 if (y) {
6011 flag = true;
6012 } else {
6013 flag = false;
6014 }
6015 } else {
6016 flag = false;
6017 }
6018
6019 cartesian_tra_emxFree_boolean_T(&x);
6020 return flag;
6021}
6022
6023static void cartesian_trajectory_planne_inv(const
6024 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y)
6025{
6026 int32_T n;
6027 emxArray_int32_T_cartesian_tr_T *p;
6028 int32_T c;
6029 emxArray_real_T_cartesian_tra_T *c_A;
6030 emxArray_int32_T_cartesian_tr_T *b_ipiv;
6031 int32_T info;
6032 int32_T n_0;
6033 int32_T yk;
6034 emxArray_real_T_cartesian_tra_T *y_0;
6035 if ((x->size[0] == 0) || (x->size[1] == 0)) {
6036 info = y->size[0] * y->size[1];
6037 y->size[0] = x->size[0];
6038 y->size[1] = x->size[1];
6039 cartes_emxEnsureCapacity_real_T(y, info);
6040 n_0 = x->size[0] * x->size[1] - 1;
6041 for (info = 0; info <= n_0; info++) {
6042 y->data[info] = x->data[info];
6043 }
6044 } else {
6045 n = x->size[0];
6046 info = y->size[0] * y->size[1];
6047 y->size[0] = x->size[0];
6048 y->size[1] = x->size[1];
6049 cartes_emxEnsureCapacity_real_T(y, info);
6050 n_0 = x->size[0] * x->size[1] - 1;
6051 for (info = 0; info <= n_0; info++) {
6052 y->data[info] = 0.0;
6053 }
6054
6055 cartesian_traje_emxInit_int32_T(&p, 2);
6056 cartesian_trajec_emxInit_real_T(&c_A, 2);
6057 cartesian_traje_emxInit_int32_T(&b_ipiv, 2);
6058 cartesian_trajectory_pl_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
6059 b_ipiv, &info);
6060 if (x->size[0] < 1) {
6061 n_0 = 0;
6062 } else {
6063 n_0 = x->size[0];
6064 }
6065
6066 info = p->size[0] * p->size[1];
6067 p->size[0] = 1;
6068 p->size[1] = n_0;
6069 carte_emxEnsureCapacity_int32_T(p, info);
6070 if (n_0 > 0) {
6071 p->data[0] = 1;
6072 yk = 1;
6073 for (info = 2; info <= n_0; info++) {
6074 yk++;
6075 p->data[info - 1] = yk;
6076 }
6077 }
6078
6079 n_0 = b_ipiv->size[1] - 1;
6080 for (info = 0; info <= n_0; info++) {
6081 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
6082 yk = p->data[b_ipiv->data[info] - 1];
6083 p->data[b_ipiv->data[info] - 1] = p->data[info];
6084 p->data[info] = yk;
6085 }
6086 }
6087
6088 cartesian_traje_emxFree_int32_T(&b_ipiv);
6089 for (info = 0; info < n; info++) {
6090 c = p->data[info] - 1;
6091 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
6092 for (n_0 = info + 1; n_0 <= n; n_0++) {
6093 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
6094 for (yk = n_0 + 1; yk <= n; yk++) {
6095 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
6096 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
6097 }
6098 }
6099 }
6100 }
6101
6102 cartesian_traje_emxFree_int32_T(&p);
6103 cartesian_trajec_emxInit_real_T(&y_0, 2);
6104 info = y_0->size[0] * y_0->size[1];
6105 y_0->size[0] = y->size[0];
6106 y_0->size[1] = y->size[1];
6107 cartes_emxEnsureCapacity_real_T(y_0, info);
6108 n_0 = y->size[0] * y->size[1];
6109 for (info = 0; info < n_0; info++) {
6110 y_0->data[info] = y->data[info];
6111 }
6112
6113 cartesian_trajectory_plan_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
6114 x->size[0], y);
6115 cartesian_trajec_emxFree_real_T(&y_0);
6116 cartesian_trajec_emxFree_real_T(&c_A);
6117 }
6118}
6119
6120static void cartesian_trajectory_plann_diag(const
6121 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d)
6122{
6123 int32_T u0;
6124 int32_T u1;
6125 if ((v->size[0] == 1) && (v->size[1] == 1)) {
6126 u0 = d->size[0];
6127 d->size[0] = 1;
6128 cartes_emxEnsureCapacity_real_T(d, u0);
6129 d->data[0] = v->data[0];
6130 } else {
6131 if (0 < v->size[1]) {
6132 u0 = v->size[0];
6133 u1 = v->size[1];
6134 if (u0 < u1) {
6135 u1 = u0;
6136 }
6137 } else {
6138 u1 = 0;
6139 }
6140
6141 u0 = d->size[0];
6142 d->size[0] = u1;
6143 cartes_emxEnsureCapacity_real_T(d, u0);
6144 for (u0 = 0; u0 < u1; u0++) {
6145 d->data[u0] = v->data[v->size[0] * u0 + u0];
6146 }
6147 }
6148}
6149
6150static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x)
6151{
6152 int32_T nx;
6153 int32_T b_k;
6154 nx = x->size[0] - 1;
6155 for (b_k = 0; b_k <= nx; b_k++) {
6156 x->data[b_k] = sqrt(x->data[b_k]);
6157 }
6158}
6159
6160static boolean_T cartesian_trajectory_planne_any(const
6161 emxArray_boolean_T_cartesian__T *x)
6162{
6163 boolean_T y;
6164 int32_T ix;
6165 boolean_T exitg1;
6166 y = false;
6167 ix = 0;
6168 exitg1 = false;
6169 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6170 if (!x->data[ix]) {
6171 ix++;
6172 } else {
6173 y = true;
6174 exitg1 = true;
6175 }
6176 }
6177
6178 return y;
6179}
6180
6181static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36])
6182{
6183 emxArray_real_T_cartesian_tra_T *b_x;
6184 boolean_T exitg1;
6185 cartesian_trajectory_planner_B.c_A_size_idx_0 = 6;
6186 cartesian_trajectory_planner_B.c_A_size_idx_1 = 6;
6187 memcpy(&cartesian_trajectory_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));
6188 cartesian_trajectory_planner_B.b_info = 0;
6189 cartesian_trajectory_planner_B.b_j_i = 1;
6190 cartesian_trajec_emxInit_real_T(&b_x, 2);
6191 exitg1 = false;
6192 while ((!exitg1) && (cartesian_trajectory_planner_B.b_j_i - 1 < 6)) {
6193 cartesian_trajectory_planner_B.jm1 = cartesian_trajectory_planner_B.b_j_i -
6194 2;
6195 cartesian_trajectory_planner_B.idxAjj =
6196 ((cartesian_trajectory_planner_B.b_j_i - 1) * 6 +
6197 cartesian_trajectory_planner_B.b_j_i) - 1;
6198 cartesian_trajectory_planner_B.ssq = 0.0;
6199 if (cartesian_trajectory_planner_B.b_j_i - 1 >= 1) {
6200 cartesian_trajectory_planner_B.ix_f = cartesian_trajectory_planner_B.b_j_i
6201 - 1;
6202 cartesian_trajectory_planner_B.iy = cartesian_trajectory_planner_B.b_j_i -
6203 1;
6204 for (cartesian_trajectory_planner_B.k_i = 0;
6205 cartesian_trajectory_planner_B.k_i <=
6206 cartesian_trajectory_planner_B.jm1;
6207 cartesian_trajectory_planner_B.k_i++) {
6208 cartesian_trajectory_planner_B.ssq +=
6209 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_f]
6210 * cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy];
6211 cartesian_trajectory_planner_B.ix_f += 6;
6212 cartesian_trajectory_planner_B.iy += 6;
6213 }
6214 }
6215
6216 cartesian_trajectory_planner_B.ssq =
6217 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
6218 - cartesian_trajectory_planner_B.ssq;
6219 if (cartesian_trajectory_planner_B.ssq > 0.0) {
6220 cartesian_trajectory_planner_B.ssq = sqrt
6221 (cartesian_trajectory_planner_B.ssq);
6222 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
6223 = cartesian_trajectory_planner_B.ssq;
6224 if (cartesian_trajectory_planner_B.b_j_i < 6) {
6225 if (cartesian_trajectory_planner_B.b_j_i - 1 != 0) {
6226 cartesian_trajectory_planner_B.ix_f =
6227 cartesian_trajectory_planner_B.b_j_i - 1;
6228 cartesian_trajectory_planner_B.jm1 =
6229 (cartesian_trajectory_planner_B.b_j_i - 2) * 6 +
6230 cartesian_trajectory_planner_B.b_j_i;
6231 for (cartesian_trajectory_planner_B.k_i =
6232 cartesian_trajectory_planner_B.b_j_i + 1;
6233 cartesian_trajectory_planner_B.k_i <=
6234 cartesian_trajectory_planner_B.jm1 + 1;
6235 cartesian_trajectory_planner_B.k_i += 6) {
6236 cartesian_trajectory_planner_B.c =
6237 -cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_f];
6238 cartesian_trajectory_planner_B.iy =
6239 cartesian_trajectory_planner_B.idxAjj + 1;
6240 cartesian_trajectory_planner_B.d_n =
6241 cartesian_trajectory_planner_B.k_i -
6242 cartesian_trajectory_planner_B.b_j_i;
6243 for (cartesian_trajectory_planner_B.ia =
6244 cartesian_trajectory_planner_B.k_i;
6245 cartesian_trajectory_planner_B.ia <=
6246 cartesian_trajectory_planner_B.d_n + 5;
6247 cartesian_trajectory_planner_B.ia++) {
6248 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy]
6249 +=
6250 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ia
6251 - 1] * cartesian_trajectory_planner_B.c;
6252 cartesian_trajectory_planner_B.iy++;
6253 }
6254
6255 cartesian_trajectory_planner_B.ix_f += 6;
6256 }
6257 }
6258
6259 cartesian_trajectory_planner_B.ssq = 1.0 /
6260 cartesian_trajectory_planner_B.ssq;
6261 cartesian_trajectory_planner_B.ix_f = b_x->size[0] * b_x->size[1];
6262 b_x->size[0] = 6;
6263 b_x->size[1] = 6;
6264 cartes_emxEnsureCapacity_real_T(b_x, cartesian_trajectory_planner_B.ix_f);
6265 cartesian_trajectory_planner_B.c_A_size_idx_0 =
6266 cartesian_trajectory_planner_B.c_A_size_idx_0 *
6267 cartesian_trajectory_planner_B.c_A_size_idx_1 - 1;
6268 for (cartesian_trajectory_planner_B.ix_f = 0;
6269 cartesian_trajectory_planner_B.ix_f <=
6270 cartesian_trajectory_planner_B.c_A_size_idx_0;
6271 cartesian_trajectory_planner_B.ix_f++) {
6272 b_x->data[cartesian_trajectory_planner_B.ix_f] =
6273 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_f];
6274 }
6275
6276 cartesian_trajectory_planner_B.jm1 =
6277 cartesian_trajectory_planner_B.idxAjj -
6278 cartesian_trajectory_planner_B.b_j_i;
6279 for (cartesian_trajectory_planner_B.k_i =
6280 cartesian_trajectory_planner_B.idxAjj + 2;
6281 cartesian_trajectory_planner_B.k_i <=
6282 cartesian_trajectory_planner_B.jm1 + 7;
6283 cartesian_trajectory_planner_B.k_i++) {
6284 b_x->data[cartesian_trajectory_planner_B.k_i - 1] *=
6285 cartesian_trajectory_planner_B.ssq;
6286 }
6287
6288 cartesian_trajectory_planner_B.c_A_size_idx_0 = b_x->size[0];
6289 cartesian_trajectory_planner_B.c_A_size_idx_1 = b_x->size[1];
6290 for (cartesian_trajectory_planner_B.ix_f = 0;
6291 cartesian_trajectory_planner_B.ix_f < 36;
6292 cartesian_trajectory_planner_B.ix_f++) {
6293 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_f]
6294 = b_x->data[cartesian_trajectory_planner_B.ix_f];
6295 }
6296 }
6297
6298 cartesian_trajectory_planner_B.b_j_i++;
6299 } else {
6300 cartesian_trajectory_planner_B.b_info =
6301 cartesian_trajectory_planner_B.b_j_i;
6302 exitg1 = true;
6303 }
6304 }
6305
6306 cartesian_trajec_emxFree_real_T(&b_x);
6307 return cartesian_trajectory_planner_B.b_info == 0;
6308}
6309
6310static boolean_T DampedBFGSwGradientProjectio_as(const
6311 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])
6312{
6313 boolean_T flag;
6314 emxArray_real_T_cartesian_tra_T *b;
6315 emxArray_real_T_cartesian_tra_T *c;
6316 int32_T m;
6317 int32_T inner;
6318 int32_T b_i;
6319 int32_T loop_ub;
6320 emxArray_boolean_T_cartesian__T *c_0;
6321 cartesian_trajec_emxInit_real_T(&b, 2);
6322 cartesian_trajec_emxInit_real_T(&c, 1);
6323 cartesian_tra_emxInit_boolean_T(&c_0, 1);
6324 if (obj->ConstraintsOn) {
6325 b_i = b->size[0] * b->size[1];
6326 b->size[0] = obj->ConstraintMatrix->size[0];
6327 b->size[1] = obj->ConstraintMatrix->size[1];
6328 cartes_emxEnsureCapacity_real_T(b, b_i);
6329 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
6330 1;
6331 for (b_i = 0; b_i <= loop_ub; b_i++) {
6332 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
6333 }
6334
6335 m = b->size[1] - 1;
6336 inner = b->size[0] - 1;
6337 b_i = c->size[0];
6338 c->size[0] = b->size[1];
6339 cartes_emxEnsureCapacity_real_T(c, b_i);
6340 for (b_i = 0; b_i <= m; b_i++) {
6341 c->data[b_i] = 0.0;
6342 }
6343
6344 for (b_i = 0; b_i <= inner; b_i++) {
6345 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
6346 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
6347 }
6348 }
6349
6350 b_i = c_0->size[0];
6351 c_0->size[0] = c->size[0];
6352 car_emxEnsureCapacity_boolean_T(c_0, b_i);
6353 loop_ub = c->size[0];
6354 for (b_i = 0; b_i < loop_ub; b_i++) {
6355 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
6356 1.4901161193847656E-8);
6357 }
6358
6359 if (cartesian_trajectory_planne_any(c_0)) {
6360 flag = true;
6361 } else {
6362 flag = false;
6363 }
6364 } else {
6365 flag = false;
6366 }
6367
6368 cartesian_tra_emxFree_boolean_T(&c_0);
6369 cartesian_trajec_emxFree_real_T(&c);
6370 cartesian_trajec_emxFree_real_T(&b);
6371 return flag;
6372}
6373
6374static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
6375 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
6376 *iter)
6377{
6378 emxArray_real_T_cartesian_tra_T *unusedU1;
6379 emxArray_real_T_cartesian_tra_T *grad;
6380 emxArray_boolean_T_cartesian__T *activeSet;
6381 emxArray_real_T_cartesian_tra_T *A;
6382 emxArray_real_T_cartesian_tra_T *alpha;
6383 emxArray_real_T_cartesian_tra_T *AIn;
6384 emxArray_real_T_cartesian_tra_T *L;
6385 f_robotics_manip_internal_IKE_T *b;
6386 f_robotics_manip_internal_IKE_T *c;
6387 f_robotics_manip_internal_IKE_T *d;
6388 emxArray_int32_T_cartesian_tr_T *cb;
6389 emxArray_int32_T_cartesian_tr_T *db;
6390 emxArray_int32_T_cartesian_tr_T *eb;
6391 emxArray_int32_T_cartesian_tr_T *fb;
6392 emxArray_int32_T_cartesian_tr_T *gb;
6393 f_robotics_manip_internal_IKE_T *args;
6394 emxArray_real_T_cartesian_tra_T *a;
6395 emxArray_int32_T_cartesian_tr_T *ii;
6396 emxArray_real_T_cartesian_tra_T *y;
6397 emxArray_int32_T_cartesian_tr_T *ii_0;
6398 emxArray_real_T_cartesian_tra_T *y_0;
6399 emxArray_boolean_T_cartesian__T *x;
6400 emxArray_real_T_cartesian_tra_T *A_0;
6401 emxArray_real_T_cartesian_tra_T *A_1;
6402 emxArray_real_T_cartesian_tra_T *A_2;
6403 emxArray_real_T_cartesian_tra_T *sigma;
6404 emxArray_real_T_cartesian_tra_T *tmp;
6405 emxArray_real_T_cartesian_tra_T *tmp_0;
6406 emxArray_real_T_cartesian_tra_T *grad_0;
6407 emxArray_real_T_cartesian_tra_T *sNew;
6408 emxArray_int32_T_cartesian_tr_T *ii_1;
6409 emxArray_int32_T_cartesian_tr_T *ii_2;
6410 emxArray_real_T_cartesian_tra_T *grad_1;
6411 emxArray_real_T_cartesian_tra_T *A_3;
6412 emxArray_real_T_cartesian_tra_T *alpha_0;
6413 emxArray_int32_T_cartesian_tr_T *ii_3;
6414 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
6415 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
6416
6417 int32_T exitg1;
6418 int32_T exitg2;
6419 boolean_T exitg3;
6420 boolean_T guard1 = false;
6421 boolean_T guard2 = false;
6422 for (cartesian_trajectory_planner_B.i_n = 0;
6423 cartesian_trajectory_planner_B.i_n < 6;
6424 cartesian_trajectory_planner_B.i_n++) {
6425 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_n] =
6426 obj->SeedInternal[cartesian_trajectory_planner_B.i_n];
6427 }
6428
6429 cartesian_trajec_emxInit_real_T(&unusedU1, 2);
6430 cartesian_trajec_emxInit_real_T(&grad, 1);
6431 obj->TimeObjInternal.StartTime = ctimefun();
6432 cartesian_IKHelpers_computeCost(cartesian_trajectory_planner_B.x,
6433 obj->ExtraArgs, &cartesian_trajectory_planner_B.cost,
6434 cartesian_trajectory_planner_B.unusedU0, unusedU1, &b);
6435 obj->ExtraArgs = b;
6436 args = obj->ExtraArgs;
6437 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
6438 grad->size[0] = args->GradTemp->size[0];
6439 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i_o);
6440 cartesian_trajectory_planner_B.i_n = args->GradTemp->size[0];
6441 for (cartesian_trajectory_planner_B.b_i_o = 0;
6442 cartesian_trajectory_planner_B.b_i_o < cartesian_trajectory_planner_B.i_n;
6443 cartesian_trajectory_planner_B.b_i_o++) {
6444 grad->data[cartesian_trajectory_planner_B.b_i_o] = args->GradTemp->
6445 data[cartesian_trajectory_planner_B.b_i_o];
6446 }
6447
6448 cartesian_trajectory_planne_eye(cartesian_trajectory_planner_B.unusedU0);
6449 memcpy(&cartesian_trajectory_planner_B.H[0],
6450 &cartesian_trajectory_planner_B.unusedU0[0], 36U * sizeof(real_T));
6451 cartesian_tra_emxInit_boolean_T(&activeSet, 1);
6452 cartesian_trajec_emxInit_real_T(&A, 2);
6453 cartesian_trajec_emxInit_real_T(&alpha, 1);
6454 cartesian_traje_emxInit_int32_T(&ii, 1);
6455 if (obj->ConstraintsOn) {
6456 cartesian_trajectory_planner_B.b_i_o = A->size[0] * A->size[1];
6457 A->size[0] = obj->ConstraintMatrix->size[0];
6458 A->size[1] = obj->ConstraintMatrix->size[1];
6459 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i_o);
6460 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0] *
6461 obj->ConstraintMatrix->size[1] - 1;
6462 for (cartesian_trajectory_planner_B.b_i_o = 0;
6463 cartesian_trajectory_planner_B.b_i_o <=
6464 cartesian_trajectory_planner_B.i_n;
6465 cartesian_trajectory_planner_B.b_i_o++) {
6466 A->data[cartesian_trajectory_planner_B.b_i_o] = obj->
6467 ConstraintMatrix->data[cartesian_trajectory_planner_B.b_i_o];
6468 }
6469
6470 cartesian_trajectory_planner_B.m_hz = A->size[1] - 1;
6471 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
6472 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
6473 alpha->size[0] = A->size[1];
6474 cartes_emxEnsureCapacity_real_T(alpha, cartesian_trajectory_planner_B.b_i_o);
6475 for (cartesian_trajectory_planner_B.b_i_o = 0;
6476 cartesian_trajectory_planner_B.b_i_o <=
6477 cartesian_trajectory_planner_B.m_hz;
6478 cartesian_trajectory_planner_B.b_i_o++) {
6479 alpha->data[cartesian_trajectory_planner_B.b_i_o] = 0.0;
6480 }
6481
6482 for (cartesian_trajectory_planner_B.nx_k = 0;
6483 cartesian_trajectory_planner_B.nx_k <=
6484 cartesian_trajectory_planner_B.inner;
6485 cartesian_trajectory_planner_B.nx_k++) {
6486 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6487 cartesian_trajectory_planner_B.g_idx_0 <=
6488 cartesian_trajectory_planner_B.m_hz;
6489 cartesian_trajectory_planner_B.g_idx_0++) {
6490 alpha->data[cartesian_trajectory_planner_B.g_idx_0] += A->
6491 data[cartesian_trajectory_planner_B.g_idx_0 * A->size[0] +
6492 cartesian_trajectory_planner_B.nx_k] *
6493 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_k];
6494 }
6495 }
6496
6497 cartesian_trajectory_planner_B.b_i_o = activeSet->size[0];
6498 activeSet->size[0] = alpha->size[0];
6499 car_emxEnsureCapacity_boolean_T(activeSet,
6500 cartesian_trajectory_planner_B.b_i_o);
6501 cartesian_trajectory_planner_B.i_n = alpha->size[0];
6502 for (cartesian_trajectory_planner_B.b_i_o = 0;
6503 cartesian_trajectory_planner_B.b_i_o <
6504 cartesian_trajectory_planner_B.i_n;
6505 cartesian_trajectory_planner_B.b_i_o++) {
6506 activeSet->data[cartesian_trajectory_planner_B.b_i_o] = (alpha->
6507 data[cartesian_trajectory_planner_B.b_i_o] >= obj->ConstraintBound->
6508 data[cartesian_trajectory_planner_B.b_i_o]);
6509 }
6510
6511 cartesian_trajectory_planner_B.nx_k = activeSet->size[0] - 1;
6512 cartesian_trajectory_planner_B.idx = 0;
6513 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6514 cartesian_trajectory_planner_B.g_idx_0 <=
6515 cartesian_trajectory_planner_B.nx_k;
6516 cartesian_trajectory_planner_B.g_idx_0++) {
6517 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
6518 cartesian_trajectory_planner_B.idx++;
6519 }
6520 }
6521
6522 cartesian_trajectory_planner_B.b_i_o = ii->size[0];
6523 ii->size[0] = cartesian_trajectory_planner_B.idx;
6524 carte_emxEnsureCapacity_int32_T(ii, cartesian_trajectory_planner_B.b_i_o);
6525 cartesian_trajectory_planner_B.b_i_o = 0;
6526 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6527 cartesian_trajectory_planner_B.g_idx_0 <=
6528 cartesian_trajectory_planner_B.nx_k;
6529 cartesian_trajectory_planner_B.g_idx_0++) {
6530 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
6531 ii->data[cartesian_trajectory_planner_B.b_i_o] =
6532 cartesian_trajectory_planner_B.g_idx_0 + 1;
6533 cartesian_trajectory_planner_B.b_i_o++;
6534 }
6535 }
6536
6537 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0];
6538 cartesian_trajectory_planner_B.b_i_o = A->size[0] * A->size[1];
6539 A->size[0] = cartesian_trajectory_planner_B.i_n;
6540 A->size[1] = ii->size[0];
6541 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i_o);
6542 cartesian_trajectory_planner_B.n_b = ii->size[0];
6543 for (cartesian_trajectory_planner_B.b_i_o = 0;
6544 cartesian_trajectory_planner_B.b_i_o <
6545 cartesian_trajectory_planner_B.n_b;
6546 cartesian_trajectory_planner_B.b_i_o++) {
6547 for (cartesian_trajectory_planner_B.idx = 0;
6548 cartesian_trajectory_planner_B.idx <
6549 cartesian_trajectory_planner_B.i_n;
6550 cartesian_trajectory_planner_B.idx++) {
6551 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
6552 cartesian_trajectory_planner_B.b_i_o] = obj->ConstraintMatrix->data
6553 [(ii->data[cartesian_trajectory_planner_B.b_i_o] - 1) *
6554 obj->ConstraintMatrix->size[0] + cartesian_trajectory_planner_B.idx];
6555 }
6556 }
6557 } else {
6558 cartesian_trajectory_planner_B.g_idx_0 = obj->ConstraintBound->size[0];
6559 cartesian_trajectory_planner_B.b_i_o = activeSet->size[0];
6560 activeSet->size[0] = cartesian_trajectory_planner_B.g_idx_0;
6561 car_emxEnsureCapacity_boolean_T(activeSet,
6562 cartesian_trajectory_planner_B.b_i_o);
6563 for (cartesian_trajectory_planner_B.b_i_o = 0;
6564 cartesian_trajectory_planner_B.b_i_o <
6565 cartesian_trajectory_planner_B.g_idx_0;
6566 cartesian_trajectory_planner_B.b_i_o++) {
6567 activeSet->data[cartesian_trajectory_planner_B.b_i_o] = false;
6568 }
6569
6570 A->size[0] = 6;
6571 A->size[1] = 0;
6572 }
6573
6574 cartesian_trajectory_planner_B.j = A->size[1] - 1;
6575 cartesian_trajec_emxInit_real_T(&AIn, 2);
6576 cartesian_trajec_emxInit_real_T(&A_0, 2);
6577 cartesian_trajec_emxInit_real_T(&A_1, 1);
6578 for (cartesian_trajectory_planner_B.nx_k = 0;
6579 cartesian_trajectory_planner_B.nx_k <= cartesian_trajectory_planner_B.j;
6580 cartesian_trajectory_planner_B.nx_k++) {
6581 cartesian_trajectory_planner_B.i_n = A->size[0];
6582 cartesian_trajectory_planner_B.b_i_o = A_0->size[0] * A_0->size[1];
6583 A_0->size[0] = 1;
6584 A_0->size[1] = cartesian_trajectory_planner_B.i_n;
6585 cartes_emxEnsureCapacity_real_T(A_0, cartesian_trajectory_planner_B.b_i_o);
6586 for (cartesian_trajectory_planner_B.b_i_o = 0;
6587 cartesian_trajectory_planner_B.b_i_o <
6588 cartesian_trajectory_planner_B.i_n;
6589 cartesian_trajectory_planner_B.b_i_o++) {
6590 A_0->data[cartesian_trajectory_planner_B.b_i_o] = A->data[A->size[0] *
6591 cartesian_trajectory_planner_B.nx_k +
6592 cartesian_trajectory_planner_B.b_i_o];
6593 }
6594
6595 cartesian_trajectory_planner_B.i_n = A->size[0];
6596 cartesian_trajectory_planner_B.b_i_o = A_1->size[0];
6597 A_1->size[0] = cartesian_trajectory_planner_B.i_n;
6598 cartes_emxEnsureCapacity_real_T(A_1, cartesian_trajectory_planner_B.b_i_o);
6599 for (cartesian_trajectory_planner_B.b_i_o = 0;
6600 cartesian_trajectory_planner_B.b_i_o <
6601 cartesian_trajectory_planner_B.i_n;
6602 cartesian_trajectory_planner_B.b_i_o++) {
6603 A_1->data[cartesian_trajectory_planner_B.b_i_o] = A->data[A->size[0] *
6604 cartesian_trajectory_planner_B.nx_k +
6605 cartesian_trajectory_planner_B.b_i_o];
6606 }
6607
6608 cartesian_trajectory_planner_B.A_d = 0.0;
6609 for (cartesian_trajectory_planner_B.b_i_o = 0;
6610 cartesian_trajectory_planner_B.b_i_o < 6;
6611 cartesian_trajectory_planner_B.b_i_o++) {
6612 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o] =
6613 0.0;
6614 for (cartesian_trajectory_planner_B.idx = 0;
6615 cartesian_trajectory_planner_B.idx < 6;
6616 cartesian_trajectory_planner_B.idx++) {
6617 cartesian_trajectory_planner_B.s_e = cartesian_trajectory_planner_B.H[6 *
6618 cartesian_trajectory_planner_B.b_i_o +
6619 cartesian_trajectory_planner_B.idx] * A_0->
6620 data[cartesian_trajectory_planner_B.idx] +
6621 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o];
6622 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o] =
6623 cartesian_trajectory_planner_B.s_e;
6624 }
6625
6626 cartesian_trajectory_planner_B.A_d +=
6627 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o] *
6628 A_1->data[cartesian_trajectory_planner_B.b_i_o];
6629 }
6630
6631 cartesian_trajectory_planner_B.s_e = 1.0 /
6632 cartesian_trajectory_planner_B.A_d;
6633 for (cartesian_trajectory_planner_B.b_i_o = 0;
6634 cartesian_trajectory_planner_B.b_i_o < 36;
6635 cartesian_trajectory_planner_B.b_i_o++) {
6636 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i_o] =
6637 cartesian_trajectory_planner_B.s_e *
6638 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i_o];
6639 }
6640
6641 cartesian_trajectory_planner_B.i_n = A->size[0];
6642 cartesian_trajectory_planner_B.n_b = A->size[0];
6643 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
6644 AIn->size[0] = cartesian_trajectory_planner_B.i_n;
6645 AIn->size[1] = cartesian_trajectory_planner_B.n_b;
6646 cartes_emxEnsureCapacity_real_T(AIn, cartesian_trajectory_planner_B.b_i_o);
6647 for (cartesian_trajectory_planner_B.b_i_o = 0;
6648 cartesian_trajectory_planner_B.b_i_o <
6649 cartesian_trajectory_planner_B.n_b;
6650 cartesian_trajectory_planner_B.b_i_o++) {
6651 for (cartesian_trajectory_planner_B.idx = 0;
6652 cartesian_trajectory_planner_B.idx <
6653 cartesian_trajectory_planner_B.i_n;
6654 cartesian_trajectory_planner_B.idx++) {
6655 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
6656 cartesian_trajectory_planner_B.b_i_o] = A->data[A->size[0] *
6657 cartesian_trajectory_planner_B.nx_k +
6658 cartesian_trajectory_planner_B.idx] * A->data[A->size[0] *
6659 cartesian_trajectory_planner_B.nx_k +
6660 cartesian_trajectory_planner_B.b_i_o];
6661 }
6662 }
6663
6664 cartesian_trajectory_planner_B.n_b = AIn->size[1] - 1;
6665 cartesian_trajectory_planner_B.b_i_o = unusedU1->size[0] * unusedU1->size[1];
6666 unusedU1->size[0] = 6;
6667 unusedU1->size[1] = AIn->size[1];
6668 cartes_emxEnsureCapacity_real_T(unusedU1,
6669 cartesian_trajectory_planner_B.b_i_o);
6670 for (cartesian_trajectory_planner_B.idx = 0;
6671 cartesian_trajectory_planner_B.idx <=
6672 cartesian_trajectory_planner_B.n_b; cartesian_trajectory_planner_B.idx
6673 ++) {
6674 cartesian_trajectory_planner_B.coffset =
6675 cartesian_trajectory_planner_B.idx * 6 - 1;
6676 cartesian_trajectory_planner_B.boffset =
6677 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
6678 for (cartesian_trajectory_planner_B.b_i_o = 0;
6679 cartesian_trajectory_planner_B.b_i_o < 6;
6680 cartesian_trajectory_planner_B.b_i_o++) {
6681 cartesian_trajectory_planner_B.s_e = 0.0;
6682 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6683 cartesian_trajectory_planner_B.g_idx_0 < 6;
6684 cartesian_trajectory_planner_B.g_idx_0++) {
6685 cartesian_trajectory_planner_B.s_e +=
6686 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.g_idx_0
6687 * 6 + cartesian_trajectory_planner_B.b_i_o] * AIn->data
6688 [(cartesian_trajectory_planner_B.boffset +
6689 cartesian_trajectory_planner_B.g_idx_0) + 1];
6690 }
6691
6692 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
6693 cartesian_trajectory_planner_B.b_i_o) + 1] =
6694 cartesian_trajectory_planner_B.s_e;
6695 }
6696 }
6697
6698 for (cartesian_trajectory_planner_B.b_i_o = 0;
6699 cartesian_trajectory_planner_B.b_i_o < 6;
6700 cartesian_trajectory_planner_B.b_i_o++) {
6701 for (cartesian_trajectory_planner_B.idx = 0;
6702 cartesian_trajectory_planner_B.idx < 6;
6703 cartesian_trajectory_planner_B.idx++) {
6704 cartesian_trajectory_planner_B.s_e = 0.0;
6705 for (cartesian_trajectory_planner_B.i_n = 0;
6706 cartesian_trajectory_planner_B.i_n < 6;
6707 cartesian_trajectory_planner_B.i_n++) {
6708 cartesian_trajectory_planner_B.s_e += unusedU1->data[6 *
6709 cartesian_trajectory_planner_B.i_n +
6710 cartesian_trajectory_planner_B.b_i_o] *
6711 cartesian_trajectory_planner_B.H[6 *
6712 cartesian_trajectory_planner_B.idx +
6713 cartesian_trajectory_planner_B.i_n];
6714 }
6715
6716 cartesian_trajectory_planner_B.idxl = 6 *
6717 cartesian_trajectory_planner_B.idx +
6718 cartesian_trajectory_planner_B.b_i_o;
6719 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl] =
6720 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
6721 - cartesian_trajectory_planner_B.s_e;
6722 }
6723 }
6724
6725 memcpy(&cartesian_trajectory_planner_B.H[0],
6726 &cartesian_trajectory_planner_B.H_m[0], 36U * sizeof(real_T));
6727 }
6728
6729 cartesian_trajec_emxFree_real_T(&A_1);
6730 cartesian_trajec_emxFree_real_T(&A_0);
6731 for (cartesian_trajectory_planner_B.i_n = 0;
6732 cartesian_trajectory_planner_B.i_n < 6;
6733 cartesian_trajectory_planner_B.i_n++) {
6734 xSol[cartesian_trajectory_planner_B.i_n] =
6735 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_n];
6736 }
6737
6738 cartesian_trajectory_planner_B.A_d = obj->MaxNumIterationInternal;
6739 cartesian_trajectory_planner_B.g_idx_0 = 0;
6740 cartesian_trajec_emxInit_real_T(&L, 1);
6741 cartesian_traje_emxInit_int32_T(&cb, 1);
6742 cartesian_traje_emxInit_int32_T(&db, 1);
6743 cartesian_traje_emxInit_int32_T(&eb, 1);
6744 cartesian_traje_emxInit_int32_T(&fb, 1);
6745 cartesian_traje_emxInit_int32_T(&gb, 1);
6746 cartesian_trajec_emxInit_real_T(&a, 2);
6747 cartesian_trajec_emxInit_real_T(&y, 1);
6748 cartesian_traje_emxInit_int32_T(&ii_0, 1);
6749 cartesian_trajec_emxInit_real_T(&y_0, 2);
6750 cartesian_tra_emxInit_boolean_T(&x, 1);
6751 cartesian_trajec_emxInit_real_T(&A_2, 2);
6752 cartesian_trajec_emxInit_real_T(&sigma, 2);
6753 cartesian_trajec_emxInit_real_T(&tmp, 2);
6754 cartesian_trajec_emxInit_real_T(&tmp_0, 2);
6755 cartesian_trajec_emxInit_real_T(&grad_0, 2);
6756 cartesian_trajec_emxInit_real_T(&sNew, 2);
6757 cartesian_traje_emxInit_int32_T(&ii_1, 1);
6758 cartesian_traje_emxInit_int32_T(&ii_2, 1);
6759 cartesian_trajec_emxInit_real_T(&grad_1, 2);
6760 cartesian_trajec_emxInit_real_T(&A_3, 2);
6761 cartesian_trajec_emxInit_real_T(&alpha_0, 2);
6762 cartesian_traje_emxInit_int32_T(&ii_3, 1);
6763 do {
6764 exitg2 = 0;
6765 if (cartesian_trajectory_planner_B.g_idx_0 <= static_cast<int32_T>
6766 (cartesian_trajectory_planner_B.A_d) - 1) {
6767 cartesian_trajectory_planner_B.s_e = SystemTimeProvider_getElapsedTi
6768 (&obj->TimeObjInternal);
6769 cartesian_trajectory_planner_B.flag = (cartesian_trajectory_planner_B.s_e >
6770 obj->MaxTimeInternal);
6771 if (cartesian_trajectory_planner_B.flag) {
6772 *exitFlag = TimeLimitExceeded;
6773 args = obj->ExtraArgs;
6774 for (cartesian_trajectory_planner_B.b_i_o = 0;
6775 cartesian_trajectory_planner_B.b_i_o < 36;
6776 cartesian_trajectory_planner_B.b_i_o++) {
6777 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
6778 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
6779 }
6780
6781 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
6782 grad->size[0] = args->ErrTemp->size[0];
6783 cartes_emxEnsureCapacity_real_T(grad,
6784 cartesian_trajectory_planner_B.b_i_o);
6785 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
6786 for (cartesian_trajectory_planner_B.b_i_o = 0;
6787 cartesian_trajectory_planner_B.b_i_o <
6788 cartesian_trajectory_planner_B.i_n;
6789 cartesian_trajectory_planner_B.b_i_o++) {
6790 grad->data[cartesian_trajectory_planner_B.b_i_o] = args->ErrTemp->
6791 data[cartesian_trajectory_planner_B.b_i_o];
6792 }
6793
6794 for (cartesian_trajectory_planner_B.b_i_o = 0;
6795 cartesian_trajectory_planner_B.b_i_o < 6;
6796 cartesian_trajectory_planner_B.b_i_o++) {
6797 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
6798 = 0.0;
6799 for (cartesian_trajectory_planner_B.idx = 0;
6800 cartesian_trajectory_planner_B.idx < 6;
6801 cartesian_trajectory_planner_B.idx++) {
6802 cartesian_trajectory_planner_B.A_d =
6803 cartesian_trajectory_planner_B.unusedU0[6 *
6804 cartesian_trajectory_planner_B.idx +
6805 cartesian_trajectory_planner_B.b_i_o] * grad->
6806 data[cartesian_trajectory_planner_B.idx] +
6807 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
6808 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
6809 = cartesian_trajectory_planner_B.A_d;
6810 }
6811 }
6812
6813 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
6814 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
6815 1.0;
6816 exitg2 = 1;
6817 } else {
6818 if ((A->size[0] == 0) || (A->size[1] == 0)) {
6819 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
6820 alpha->size[0] = 1;
6821 cartes_emxEnsureCapacity_real_T(alpha,
6822 cartesian_trajectory_planner_B.b_i_o);
6823 alpha->data[0] = 0.0;
6824 } else {
6825 cartesian_trajectory_planner_B.m_hz = A->size[1] - 1;
6826 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
6827 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
6828 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
6829 AIn->size[0] = A->size[1];
6830 AIn->size[1] = A->size[1];
6831 cartes_emxEnsureCapacity_real_T(AIn,
6832 cartesian_trajectory_planner_B.b_i_o);
6833 for (cartesian_trajectory_planner_B.idx = 0;
6834 cartesian_trajectory_planner_B.idx <=
6835 cartesian_trajectory_planner_B.n_b;
6836 cartesian_trajectory_planner_B.idx++) {
6837 cartesian_trajectory_planner_B.coffset =
6838 (cartesian_trajectory_planner_B.m_hz + 1) *
6839 cartesian_trajectory_planner_B.idx - 1;
6840 cartesian_trajectory_planner_B.boffset =
6841 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
6842 for (cartesian_trajectory_planner_B.b_i_o = 0;
6843 cartesian_trajectory_planner_B.b_i_o <=
6844 cartesian_trajectory_planner_B.m_hz;
6845 cartesian_trajectory_planner_B.b_i_o++) {
6846 AIn->data[(cartesian_trajectory_planner_B.coffset +
6847 cartesian_trajectory_planner_B.b_i_o) + 1] = 0.0;
6848 }
6849
6850 for (cartesian_trajectory_planner_B.nx_k = 0;
6851 cartesian_trajectory_planner_B.nx_k <=
6852 cartesian_trajectory_planner_B.inner;
6853 cartesian_trajectory_planner_B.nx_k++) {
6854 cartesian_trajectory_planner_B.s_e = A->data
6855 [(cartesian_trajectory_planner_B.boffset +
6856 cartesian_trajectory_planner_B.nx_k) + 1];
6857 for (cartesian_trajectory_planner_B.j = 0;
6858 cartesian_trajectory_planner_B.j <=
6859 cartesian_trajectory_planner_B.m_hz;
6860 cartesian_trajectory_planner_B.j++) {
6861 cartesian_trajectory_planner_B.b_i_o =
6862 (cartesian_trajectory_planner_B.coffset +
6863 cartesian_trajectory_planner_B.j) + 1;
6864 AIn->data[cartesian_trajectory_planner_B.b_i_o] += A->
6865 data[cartesian_trajectory_planner_B.j * A->size[0] +
6866 cartesian_trajectory_planner_B.nx_k] *
6867 cartesian_trajectory_planner_B.s_e;
6868 }
6869 }
6870 }
6871
6872 cartesian_trajectory_planner_B.b_i_o = A_2->size[0] * A_2->size[1];
6873 A_2->size[0] = A->size[1];
6874 A_2->size[1] = A->size[0];
6875 cartes_emxEnsureCapacity_real_T(A_2,
6876 cartesian_trajectory_planner_B.b_i_o);
6877 cartesian_trajectory_planner_B.i_n = A->size[0];
6878 for (cartesian_trajectory_planner_B.b_i_o = 0;
6879 cartesian_trajectory_planner_B.b_i_o <
6880 cartesian_trajectory_planner_B.i_n;
6881 cartesian_trajectory_planner_B.b_i_o++) {
6882 cartesian_trajectory_planner_B.n_b = A->size[1];
6883 for (cartesian_trajectory_planner_B.idx = 0;
6884 cartesian_trajectory_planner_B.idx <
6885 cartesian_trajectory_planner_B.n_b;
6886 cartesian_trajectory_planner_B.idx++) {
6887 A_2->data[cartesian_trajectory_planner_B.idx + A_2->size[0] *
6888 cartesian_trajectory_planner_B.b_i_o] = A->data[A->size[0] *
6889 cartesian_trajectory_planner_B.idx +
6890 cartesian_trajectory_planner_B.b_i_o];
6891 }
6892 }
6893
6894 cartesian_trajectory_p_mldivide(AIn, A_2, a);
6895 cartesian_trajectory_planner_B.m_hz = a->size[0] - 1;
6896 cartesian_trajectory_planner_B.inner = a->size[1] - 1;
6897 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
6898 alpha->size[0] = a->size[0];
6899 cartes_emxEnsureCapacity_real_T(alpha,
6900 cartesian_trajectory_planner_B.b_i_o);
6901 for (cartesian_trajectory_planner_B.b_i_o = 0;
6902 cartesian_trajectory_planner_B.b_i_o <=
6903 cartesian_trajectory_planner_B.m_hz;
6904 cartesian_trajectory_planner_B.b_i_o++) {
6905 alpha->data[cartesian_trajectory_planner_B.b_i_o] = 0.0;
6906 }
6907
6908 for (cartesian_trajectory_planner_B.nx_k = 0;
6909 cartesian_trajectory_planner_B.nx_k <=
6910 cartesian_trajectory_planner_B.inner;
6911 cartesian_trajectory_planner_B.nx_k++) {
6912 cartesian_trajectory_planner_B.aoffset =
6913 cartesian_trajectory_planner_B.nx_k * a->size[0] - 1;
6914 for (cartesian_trajectory_planner_B.j = 0;
6915 cartesian_trajectory_planner_B.j <=
6916 cartesian_trajectory_planner_B.m_hz;
6917 cartesian_trajectory_planner_B.j++) {
6918 alpha->data[cartesian_trajectory_planner_B.j] += a->data
6919 [(cartesian_trajectory_planner_B.aoffset +
6920 cartesian_trajectory_planner_B.j) + 1] * grad->
6921 data[cartesian_trajectory_planner_B.nx_k];
6922 }
6923 }
6924 }
6925
6926 for (cartesian_trajectory_planner_B.b_i_o = 0;
6927 cartesian_trajectory_planner_B.b_i_o < 6;
6928 cartesian_trajectory_planner_B.b_i_o++) {
6929 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
6930 = 0.0;
6931 for (cartesian_trajectory_planner_B.idx = 0;
6932 cartesian_trajectory_planner_B.idx < 6;
6933 cartesian_trajectory_planner_B.idx++) {
6934 cartesian_trajectory_planner_B.b_gamma =
6935 cartesian_trajectory_planner_B.H[6 *
6936 cartesian_trajectory_planner_B.idx +
6937 cartesian_trajectory_planner_B.b_i_o] * grad->
6938 data[cartesian_trajectory_planner_B.idx] +
6939 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o];
6940 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
6941 = cartesian_trajectory_planner_B.b_gamma;
6942 }
6943 }
6944
6945 if (DampedBFGSwGradientProjection_a(obj,
6946 cartesian_trajectory_planner_B.Hg, alpha)) {
6947 *exitFlag = LocalMinimumFound;
6948 args = obj->ExtraArgs;
6949 for (cartesian_trajectory_planner_B.b_i_o = 0;
6950 cartesian_trajectory_planner_B.b_i_o < 36;
6951 cartesian_trajectory_planner_B.b_i_o++) {
6952 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
6953 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
6954 }
6955
6956 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
6957 grad->size[0] = args->ErrTemp->size[0];
6958 cartes_emxEnsureCapacity_real_T(grad,
6959 cartesian_trajectory_planner_B.b_i_o);
6960 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
6961 for (cartesian_trajectory_planner_B.b_i_o = 0;
6962 cartesian_trajectory_planner_B.b_i_o <
6963 cartesian_trajectory_planner_B.i_n;
6964 cartesian_trajectory_planner_B.b_i_o++) {
6965 grad->data[cartesian_trajectory_planner_B.b_i_o] = args->
6966 ErrTemp->data[cartesian_trajectory_planner_B.b_i_o];
6967 }
6968
6969 for (cartesian_trajectory_planner_B.b_i_o = 0;
6970 cartesian_trajectory_planner_B.b_i_o < 6;
6971 cartesian_trajectory_planner_B.b_i_o++) {
6972 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
6973 = 0.0;
6974 for (cartesian_trajectory_planner_B.idx = 0;
6975 cartesian_trajectory_planner_B.idx < 6;
6976 cartesian_trajectory_planner_B.idx++) {
6977 cartesian_trajectory_planner_B.A_d =
6978 cartesian_trajectory_planner_B.unusedU0[6 *
6979 cartesian_trajectory_planner_B.idx +
6980 cartesian_trajectory_planner_B.b_i_o] * grad->
6981 data[cartesian_trajectory_planner_B.idx] +
6982 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
6983 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
6984 = cartesian_trajectory_planner_B.A_d;
6985 }
6986 }
6987
6988 *err = cartesian_trajectory_pla_norm_a
6989 (cartesian_trajectory_planner_B.x);
6990 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
6991 1.0;
6992 exitg2 = 1;
6993 } else {
6994 guard1 = false;
6995 guard2 = false;
6996 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
6997 cartesian_trajectory_planner_B.m_hz = A->size[1] - 1;
6998 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
6999 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
7000 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
7001 AIn->size[0] = A->size[1];
7002 AIn->size[1] = A->size[1];
7003 cartes_emxEnsureCapacity_real_T(AIn,
7004 cartesian_trajectory_planner_B.b_i_o);
7005 for (cartesian_trajectory_planner_B.idx = 0;
7006 cartesian_trajectory_planner_B.idx <=
7007 cartesian_trajectory_planner_B.n_b;
7008 cartesian_trajectory_planner_B.idx++) {
7009 cartesian_trajectory_planner_B.coffset =
7010 (cartesian_trajectory_planner_B.m_hz + 1) *
7011 cartesian_trajectory_planner_B.idx - 1;
7012 cartesian_trajectory_planner_B.boffset =
7013 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
7014 for (cartesian_trajectory_planner_B.b_i_o = 0;
7015 cartesian_trajectory_planner_B.b_i_o <=
7016 cartesian_trajectory_planner_B.m_hz;
7017 cartesian_trajectory_planner_B.b_i_o++) {
7018 AIn->data[(cartesian_trajectory_planner_B.coffset +
7019 cartesian_trajectory_planner_B.b_i_o) + 1] = 0.0;
7020 }
7021
7022 for (cartesian_trajectory_planner_B.nx_k = 0;
7023 cartesian_trajectory_planner_B.nx_k <=
7024 cartesian_trajectory_planner_B.inner;
7025 cartesian_trajectory_planner_B.nx_k++) {
7026 cartesian_trajectory_planner_B.s_e = A->data
7027 [(cartesian_trajectory_planner_B.boffset +
7028 cartesian_trajectory_planner_B.nx_k) + 1];
7029 for (cartesian_trajectory_planner_B.j = 0;
7030 cartesian_trajectory_planner_B.j <=
7031 cartesian_trajectory_planner_B.m_hz;
7032 cartesian_trajectory_planner_B.j++) {
7033 cartesian_trajectory_planner_B.b_i_o =
7034 (cartesian_trajectory_planner_B.coffset +
7035 cartesian_trajectory_planner_B.j) + 1;
7036 AIn->data[cartesian_trajectory_planner_B.b_i_o] += A->
7037 data[cartesian_trajectory_planner_B.j * A->size[0] +
7038 cartesian_trajectory_planner_B.nx_k] *
7039 cartesian_trajectory_planner_B.s_e;
7040 }
7041 }
7042 }
7043
7044 cartesian_trajectory_planne_inv(AIn, a);
7045 cartesian_trajectory_plann_diag(a, L);
7046 cartesian_trajectory_pl_sqrt_as(L);
7047 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
7048 cartes_emxEnsureCapacity_real_T(alpha,
7049 cartesian_trajectory_planner_B.b_i_o);
7050 cartesian_trajectory_planner_B.i_n = alpha->size[0];
7051 for (cartesian_trajectory_planner_B.b_i_o = 0;
7052 cartesian_trajectory_planner_B.b_i_o <
7053 cartesian_trajectory_planner_B.i_n;
7054 cartesian_trajectory_planner_B.b_i_o++) {
7055 alpha->data[cartesian_trajectory_planner_B.b_i_o] /= L->
7056 data[cartesian_trajectory_planner_B.b_i_o];
7057 }
7058
7059 cartesian_trajectory_planner_B.n_b = alpha->size[0];
7060 if (alpha->size[0] <= 2) {
7061 if (alpha->size[0] == 1) {
7062 cartesian_trajectory_planner_B.s_e = alpha->data[0];
7063 cartesian_trajectory_planner_B.idxl = 0;
7064 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
7065 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
7066 cartesian_trajectory_planner_B.s_e = alpha->data[1];
7067 cartesian_trajectory_planner_B.idxl = 1;
7068 } else {
7069 cartesian_trajectory_planner_B.s_e = alpha->data[0];
7070 cartesian_trajectory_planner_B.idxl = 0;
7071 }
7072 } else {
7073 if (!rtIsNaN(alpha->data[0])) {
7074 cartesian_trajectory_planner_B.idxl = 1;
7075 } else {
7076 cartesian_trajectory_planner_B.idxl = 0;
7077 cartesian_trajectory_planner_B.b_i_o = 2;
7078 exitg3 = false;
7079 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i_o <=
7080 alpha->size[0])) {
7081 if (!rtIsNaN(alpha->data[cartesian_trajectory_planner_B.b_i_o
7082 - 1])) {
7083 cartesian_trajectory_planner_B.idxl =
7084 cartesian_trajectory_planner_B.b_i_o;
7085 exitg3 = true;
7086 } else {
7087 cartesian_trajectory_planner_B.b_i_o++;
7088 }
7089 }
7090 }
7091
7092 if (cartesian_trajectory_planner_B.idxl == 0) {
7093 cartesian_trajectory_planner_B.s_e = alpha->data[0];
7094 } else {
7095 cartesian_trajectory_planner_B.s_e = alpha->
7096 data[cartesian_trajectory_planner_B.idxl - 1];
7097 cartesian_trajectory_planner_B.nx_k =
7098 cartesian_trajectory_planner_B.idxl;
7099 for (cartesian_trajectory_planner_B.b_i_o =
7100 cartesian_trajectory_planner_B.idxl + 1;
7101 cartesian_trajectory_planner_B.b_i_o <=
7102 cartesian_trajectory_planner_B.n_b;
7103 cartesian_trajectory_planner_B.b_i_o++) {
7104 if (cartesian_trajectory_planner_B.s_e < alpha->
7105 data[cartesian_trajectory_planner_B.b_i_o - 1]) {
7106 cartesian_trajectory_planner_B.s_e = alpha->
7107 data[cartesian_trajectory_planner_B.b_i_o - 1];
7108 cartesian_trajectory_planner_B.nx_k =
7109 cartesian_trajectory_planner_B.b_i_o;
7110 }
7111 }
7112
7113 cartesian_trajectory_planner_B.idxl =
7114 cartesian_trajectory_planner_B.nx_k - 1;
7115 }
7116 }
7117
7118 if (cartesian_trajectory_pla_norm_a
7119 (cartesian_trajectory_planner_B.Hg) < 0.5 *
7120 cartesian_trajectory_planner_B.s_e) {
7121 cartesian_trajectory_planner_B.nx_k = activeSet->size[0];
7122 cartesian_trajectory_planner_B.idx = 0;
7123 cartesian_trajectory_planner_B.b_i_o = ii->size[0];
7124 ii->size[0] = activeSet->size[0];
7125 carte_emxEnsureCapacity_int32_T(ii,
7126 cartesian_trajectory_planner_B.b_i_o);
7127 cartesian_trajectory_planner_B.b_i_o = 1;
7128 exitg3 = false;
7129 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i_o - 1 <=
7130 cartesian_trajectory_planner_B.nx_k - 1)) {
7131 if (activeSet->data[cartesian_trajectory_planner_B.b_i_o - 1]) {
7132 cartesian_trajectory_planner_B.idx++;
7133 ii->data[cartesian_trajectory_planner_B.idx - 1] =
7134 cartesian_trajectory_planner_B.b_i_o;
7135 if (cartesian_trajectory_planner_B.idx >=
7136 cartesian_trajectory_planner_B.nx_k) {
7137 exitg3 = true;
7138 } else {
7139 cartesian_trajectory_planner_B.b_i_o++;
7140 }
7141 } else {
7142 cartesian_trajectory_planner_B.b_i_o++;
7143 }
7144 }
7145
7146 if (activeSet->size[0] == 1) {
7147 if (cartesian_trajectory_planner_B.idx == 0) {
7148 ii->size[0] = 0;
7149 }
7150 } else {
7151 if (1 > cartesian_trajectory_planner_B.idx) {
7152 cartesian_trajectory_planner_B.idx = 0;
7153 }
7154
7155 cartesian_trajectory_planner_B.b_i_o = ii_1->size[0];
7156 ii_1->size[0] = cartesian_trajectory_planner_B.idx;
7157 carte_emxEnsureCapacity_int32_T(ii_1,
7158 cartesian_trajectory_planner_B.b_i_o);
7159 for (cartesian_trajectory_planner_B.b_i_o = 0;
7160 cartesian_trajectory_planner_B.b_i_o <
7161 cartesian_trajectory_planner_B.idx;
7162 cartesian_trajectory_planner_B.b_i_o++) {
7163 ii_1->data[cartesian_trajectory_planner_B.b_i_o] = ii->
7164 data[cartesian_trajectory_planner_B.b_i_o];
7165 }
7166
7167 cartesian_trajectory_planner_B.b_i_o = ii->size[0];
7168 ii->size[0] = ii_1->size[0];
7169 carte_emxEnsureCapacity_int32_T(ii,
7170 cartesian_trajectory_planner_B.b_i_o);
7171 cartesian_trajectory_planner_B.i_n = ii_1->size[0];
7172 for (cartesian_trajectory_planner_B.b_i_o = 0;
7173 cartesian_trajectory_planner_B.b_i_o <
7174 cartesian_trajectory_planner_B.i_n;
7175 cartesian_trajectory_planner_B.b_i_o++) {
7176 ii->data[cartesian_trajectory_planner_B.b_i_o] = ii_1->
7177 data[cartesian_trajectory_planner_B.b_i_o];
7178 }
7179 }
7180
7181 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
7182 alpha->size[0] = ii->size[0];
7183 cartes_emxEnsureCapacity_real_T(alpha,
7184 cartesian_trajectory_planner_B.b_i_o);
7185 cartesian_trajectory_planner_B.i_n = ii->size[0];
7186 for (cartesian_trajectory_planner_B.b_i_o = 0;
7187 cartesian_trajectory_planner_B.b_i_o <
7188 cartesian_trajectory_planner_B.i_n;
7189 cartesian_trajectory_planner_B.b_i_o++) {
7190 alpha->data[cartesian_trajectory_planner_B.b_i_o] = ii->
7191 data[cartesian_trajectory_planner_B.b_i_o];
7192 }
7193
7194 activeSet->data[static_cast<int32_T>(alpha->
7195 data[cartesian_trajectory_planner_B.idxl]) - 1] = false;
7196 cartesian_trajectory_planner_B.nx_k = activeSet->size[0] - 1;
7197 cartesian_trajectory_planner_B.idx = 0;
7198 for (cartesian_trajectory_planner_B.b_i_o = 0;
7199 cartesian_trajectory_planner_B.b_i_o <=
7200 cartesian_trajectory_planner_B.nx_k;
7201 cartesian_trajectory_planner_B.b_i_o++) {
7202 if (activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7203 cartesian_trajectory_planner_B.idx++;
7204 }
7205 }
7206
7207 cartesian_trajectory_planner_B.b_i_o = eb->size[0];
7208 eb->size[0] = cartesian_trajectory_planner_B.idx;
7209 carte_emxEnsureCapacity_int32_T(eb,
7210 cartesian_trajectory_planner_B.b_i_o);
7211 cartesian_trajectory_planner_B.idx = 0;
7212 for (cartesian_trajectory_planner_B.b_i_o = 0;
7213 cartesian_trajectory_planner_B.b_i_o <=
7214 cartesian_trajectory_planner_B.nx_k;
7215 cartesian_trajectory_planner_B.b_i_o++) {
7216 if (activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7217 eb->data[cartesian_trajectory_planner_B.idx] =
7218 cartesian_trajectory_planner_B.b_i_o + 1;
7219 cartesian_trajectory_planner_B.idx++;
7220 }
7221 }
7222
7223 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0];
7224 cartesian_trajectory_planner_B.b_i_o = A->size[0] * A->size[1];
7225 A->size[0] = cartesian_trajectory_planner_B.i_n;
7226 A->size[1] = eb->size[0];
7227 cartes_emxEnsureCapacity_real_T(A,
7228 cartesian_trajectory_planner_B.b_i_o);
7229 cartesian_trajectory_planner_B.n_b = eb->size[0];
7230 for (cartesian_trajectory_planner_B.b_i_o = 0;
7231 cartesian_trajectory_planner_B.b_i_o <
7232 cartesian_trajectory_planner_B.n_b;
7233 cartesian_trajectory_planner_B.b_i_o++) {
7234 for (cartesian_trajectory_planner_B.idx = 0;
7235 cartesian_trajectory_planner_B.idx <
7236 cartesian_trajectory_planner_B.i_n;
7237 cartesian_trajectory_planner_B.idx++) {
7238 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
7239 cartesian_trajectory_planner_B.b_i_o] =
7240 obj->ConstraintMatrix->data[(eb->
7241 data[cartesian_trajectory_planner_B.b_i_o] - 1) *
7242 obj->ConstraintMatrix->size[0] +
7243 cartesian_trajectory_planner_B.idx];
7244 }
7245 }
7246
7247 cartesian_trajectory_planner_B.m_hz = A->size[1] - 1;
7248 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
7249 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
7250 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
7251 AIn->size[0] = A->size[1];
7252 AIn->size[1] = A->size[1];
7253 cartes_emxEnsureCapacity_real_T(AIn,
7254 cartesian_trajectory_planner_B.b_i_o);
7255 for (cartesian_trajectory_planner_B.idx = 0;
7256 cartesian_trajectory_planner_B.idx <=
7257 cartesian_trajectory_planner_B.n_b;
7258 cartesian_trajectory_planner_B.idx++) {
7259 cartesian_trajectory_planner_B.coffset =
7260 (cartesian_trajectory_planner_B.m_hz + 1) *
7261 cartesian_trajectory_planner_B.idx - 1;
7262 cartesian_trajectory_planner_B.boffset =
7263 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
7264 for (cartesian_trajectory_planner_B.b_i_o = 0;
7265 cartesian_trajectory_planner_B.b_i_o <=
7266 cartesian_trajectory_planner_B.m_hz;
7267 cartesian_trajectory_planner_B.b_i_o++) {
7268 AIn->data[(cartesian_trajectory_planner_B.coffset +
7269 cartesian_trajectory_planner_B.b_i_o) + 1] = 0.0;
7270 }
7271
7272 for (cartesian_trajectory_planner_B.nx_k = 0;
7273 cartesian_trajectory_planner_B.nx_k <=
7274 cartesian_trajectory_planner_B.inner;
7275 cartesian_trajectory_planner_B.nx_k++) {
7276 cartesian_trajectory_planner_B.s_e = A->data
7277 [(cartesian_trajectory_planner_B.boffset +
7278 cartesian_trajectory_planner_B.nx_k) + 1];
7279 for (cartesian_trajectory_planner_B.j = 0;
7280 cartesian_trajectory_planner_B.j <=
7281 cartesian_trajectory_planner_B.m_hz;
7282 cartesian_trajectory_planner_B.j++) {
7283 cartesian_trajectory_planner_B.b_i_o =
7284 (cartesian_trajectory_planner_B.coffset +
7285 cartesian_trajectory_planner_B.j) + 1;
7286 AIn->data[cartesian_trajectory_planner_B.b_i_o] += A->
7287 data[cartesian_trajectory_planner_B.j * A->size[0] +
7288 cartesian_trajectory_planner_B.nx_k] *
7289 cartesian_trajectory_planner_B.s_e;
7290 }
7291 }
7292 }
7293
7294 cartesian_trajectory_planner_B.b_i_o = A_3->size[0] * A_3->size[1];
7295 A_3->size[0] = A->size[1];
7296 A_3->size[1] = A->size[0];
7297 cartes_emxEnsureCapacity_real_T(A_3,
7298 cartesian_trajectory_planner_B.b_i_o);
7299 cartesian_trajectory_planner_B.i_n = A->size[0];
7300 for (cartesian_trajectory_planner_B.b_i_o = 0;
7301 cartesian_trajectory_planner_B.b_i_o <
7302 cartesian_trajectory_planner_B.i_n;
7303 cartesian_trajectory_planner_B.b_i_o++) {
7304 cartesian_trajectory_planner_B.n_b = A->size[1];
7305 for (cartesian_trajectory_planner_B.idx = 0;
7306 cartesian_trajectory_planner_B.idx <
7307 cartesian_trajectory_planner_B.n_b;
7308 cartesian_trajectory_planner_B.idx++) {
7309 A_3->data[cartesian_trajectory_planner_B.idx + A_3->size[0] *
7310 cartesian_trajectory_planner_B.b_i_o] = A->data[A->size[0] *
7311 cartesian_trajectory_planner_B.idx +
7312 cartesian_trajectory_planner_B.b_i_o];
7313 }
7314 }
7315
7316 cartesian_trajectory_p_mldivide(AIn, A_3, a);
7317 cartesian_trajectory_planner_B.m_hz = A->size[0] - 1;
7318 cartesian_trajectory_planner_B.inner = A->size[1] - 1;
7319 cartesian_trajectory_planner_B.n_b = a->size[1] - 1;
7320 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
7321 AIn->size[0] = A->size[0];
7322 AIn->size[1] = a->size[1];
7323 cartes_emxEnsureCapacity_real_T(AIn,
7324 cartesian_trajectory_planner_B.b_i_o);
7325 for (cartesian_trajectory_planner_B.idx = 0;
7326 cartesian_trajectory_planner_B.idx <=
7327 cartesian_trajectory_planner_B.n_b;
7328 cartesian_trajectory_planner_B.idx++) {
7329 cartesian_trajectory_planner_B.coffset =
7330 (cartesian_trajectory_planner_B.m_hz + 1) *
7331 cartesian_trajectory_planner_B.idx - 1;
7332 cartesian_trajectory_planner_B.boffset =
7333 cartesian_trajectory_planner_B.idx * a->size[0] - 1;
7334 for (cartesian_trajectory_planner_B.b_i_o = 0;
7335 cartesian_trajectory_planner_B.b_i_o <=
7336 cartesian_trajectory_planner_B.m_hz;
7337 cartesian_trajectory_planner_B.b_i_o++) {
7338 AIn->data[(cartesian_trajectory_planner_B.coffset +
7339 cartesian_trajectory_planner_B.b_i_o) + 1] = 0.0;
7340 }
7341
7342 for (cartesian_trajectory_planner_B.nx_k = 0;
7343 cartesian_trajectory_planner_B.nx_k <=
7344 cartesian_trajectory_planner_B.inner;
7345 cartesian_trajectory_planner_B.nx_k++) {
7346 cartesian_trajectory_planner_B.aoffset =
7347 cartesian_trajectory_planner_B.nx_k * A->size[0] - 1;
7348 cartesian_trajectory_planner_B.s_e = a->data
7349 [(cartesian_trajectory_planner_B.boffset +
7350 cartesian_trajectory_planner_B.nx_k) + 1];
7351 for (cartesian_trajectory_planner_B.j = 0;
7352 cartesian_trajectory_planner_B.j <=
7353 cartesian_trajectory_planner_B.m_hz;
7354 cartesian_trajectory_planner_B.j++) {
7355 cartesian_trajectory_planner_B.i_n =
7356 cartesian_trajectory_planner_B.j + 1;
7357 cartesian_trajectory_planner_B.b_i_o =
7358 cartesian_trajectory_planner_B.coffset +
7359 cartesian_trajectory_planner_B.i_n;
7360 AIn->data[cartesian_trajectory_planner_B.b_i_o] += A->
7361 data[cartesian_trajectory_planner_B.aoffset +
7362 cartesian_trajectory_planner_B.i_n] *
7363 cartesian_trajectory_planner_B.s_e;
7364 }
7365 }
7366 }
7367
7368 for (cartesian_trajectory_planner_B.b_i_o = 0;
7369 cartesian_trajectory_planner_B.b_i_o < 36;
7370 cartesian_trajectory_planner_B.b_i_o++) {
7371 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i_o]
7372 =
7373 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
7374 - AIn->data[cartesian_trajectory_planner_B.b_i_o];
7375 }
7376
7377 cartesian_trajectory_planner_B.s_e = alpha->
7378 data[cartesian_trajectory_planner_B.idxl];
7379 cartesian_trajectory_planner_B.b_i_o = static_cast<int32_T>
7380 (cartesian_trajectory_planner_B.s_e);
7381 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0];
7382 cartesian_trajectory_planner_B.idx = alpha->size[0];
7383 alpha->size[0] = cartesian_trajectory_planner_B.i_n;
7384 cartes_emxEnsureCapacity_real_T(alpha,
7385 cartesian_trajectory_planner_B.idx);
7386 for (cartesian_trajectory_planner_B.idx = 0;
7387 cartesian_trajectory_planner_B.idx <
7388 cartesian_trajectory_planner_B.i_n;
7389 cartesian_trajectory_planner_B.idx++) {
7390 alpha->data[cartesian_trajectory_planner_B.idx] =
7391 obj->ConstraintMatrix->data
7392 [(cartesian_trajectory_planner_B.b_i_o - 1) *
7393 obj->ConstraintMatrix->size[0] +
7394 cartesian_trajectory_planner_B.idx];
7395 }
7396
7397 cartesian_trajectory_planner_B.b_i_o = alpha_0->size[0] *
7398 alpha_0->size[1];
7399 alpha_0->size[0] = 1;
7400 alpha_0->size[1] = alpha->size[0];
7401 cartes_emxEnsureCapacity_real_T(alpha_0,
7402 cartesian_trajectory_planner_B.b_i_o);
7403 cartesian_trajectory_planner_B.i_n = alpha->size[0];
7404 for (cartesian_trajectory_planner_B.b_i_o = 0;
7405 cartesian_trajectory_planner_B.b_i_o <
7406 cartesian_trajectory_planner_B.i_n;
7407 cartesian_trajectory_planner_B.b_i_o++) {
7408 alpha_0->data[cartesian_trajectory_planner_B.b_i_o] =
7409 alpha->data[cartesian_trajectory_planner_B.b_i_o];
7410 }
7411
7412 cartesian_trajectory_planner_B.s_e = 0.0;
7413 for (cartesian_trajectory_planner_B.b_i_o = 0;
7414 cartesian_trajectory_planner_B.b_i_o < 6;
7415 cartesian_trajectory_planner_B.b_i_o++) {
7416 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
7417 = 0.0;
7418 for (cartesian_trajectory_planner_B.idx = 0;
7419 cartesian_trajectory_planner_B.idx < 6;
7420 cartesian_trajectory_planner_B.idx++) {
7421 cartesian_trajectory_planner_B.b_gamma =
7422 cartesian_trajectory_planner_B.P[6 *
7423 cartesian_trajectory_planner_B.b_i_o +
7424 cartesian_trajectory_planner_B.idx] * alpha_0->
7425 data[cartesian_trajectory_planner_B.idx] +
7426 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o];
7427 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
7428 = cartesian_trajectory_planner_B.b_gamma;
7429 }
7430
7431 cartesian_trajectory_planner_B.s_e +=
7432 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
7433 * alpha->data[cartesian_trajectory_planner_B.b_i_o];
7434 }
7435
7436 cartesian_trajectory_planner_B.s_e = 1.0 /
7437 cartesian_trajectory_planner_B.s_e;
7438 for (cartesian_trajectory_planner_B.b_i_o = 0;
7439 cartesian_trajectory_planner_B.b_i_o < 36;
7440 cartesian_trajectory_planner_B.b_i_o++) {
7441 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i_o]
7442 = cartesian_trajectory_planner_B.s_e *
7443 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i_o];
7444 }
7445
7446 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
7447 AIn->size[0] = alpha->size[0];
7448 AIn->size[1] = alpha->size[0];
7449 cartes_emxEnsureCapacity_real_T(AIn,
7450 cartesian_trajectory_planner_B.b_i_o);
7451 cartesian_trajectory_planner_B.i_n = alpha->size[0];
7452 for (cartesian_trajectory_planner_B.b_i_o = 0;
7453 cartesian_trajectory_planner_B.b_i_o <
7454 cartesian_trajectory_planner_B.i_n;
7455 cartesian_trajectory_planner_B.b_i_o++) {
7456 cartesian_trajectory_planner_B.n_b = alpha->size[0];
7457 for (cartesian_trajectory_planner_B.idx = 0;
7458 cartesian_trajectory_planner_B.idx <
7459 cartesian_trajectory_planner_B.n_b;
7460 cartesian_trajectory_planner_B.idx++) {
7461 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
7462 cartesian_trajectory_planner_B.b_i_o] = alpha->
7463 data[cartesian_trajectory_planner_B.idx] * alpha->
7464 data[cartesian_trajectory_planner_B.b_i_o];
7465 }
7466 }
7467
7468 cartesian_trajectory_planner_B.n_b = AIn->size[1] - 1;
7469 cartesian_trajectory_planner_B.b_i_o = unusedU1->size[0] *
7470 unusedU1->size[1];
7471 unusedU1->size[0] = 6;
7472 unusedU1->size[1] = AIn->size[1];
7473 cartes_emxEnsureCapacity_real_T(unusedU1,
7474 cartesian_trajectory_planner_B.b_i_o);
7475 for (cartesian_trajectory_planner_B.idx = 0;
7476 cartesian_trajectory_planner_B.idx <=
7477 cartesian_trajectory_planner_B.n_b;
7478 cartesian_trajectory_planner_B.idx++) {
7479 cartesian_trajectory_planner_B.coffset =
7480 cartesian_trajectory_planner_B.idx * 6 - 1;
7481 cartesian_trajectory_planner_B.boffset =
7482 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
7483 for (cartesian_trajectory_planner_B.b_i_o = 0;
7484 cartesian_trajectory_planner_B.b_i_o < 6;
7485 cartesian_trajectory_planner_B.b_i_o++) {
7486 cartesian_trajectory_planner_B.s_e = 0.0;
7487 for (cartesian_trajectory_planner_B.nx_k = 0;
7488 cartesian_trajectory_planner_B.nx_k < 6;
7489 cartesian_trajectory_planner_B.nx_k++) {
7490 cartesian_trajectory_planner_B.s_e +=
7491 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.nx_k
7492 * 6 + cartesian_trajectory_planner_B.b_i_o] * AIn->data
7493 [(cartesian_trajectory_planner_B.boffset +
7494 cartesian_trajectory_planner_B.nx_k) + 1];
7495 }
7496
7497 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
7498 cartesian_trajectory_planner_B.b_i_o) + 1] =
7499 cartesian_trajectory_planner_B.s_e;
7500 }
7501 }
7502
7503 for (cartesian_trajectory_planner_B.b_i_o = 0;
7504 cartesian_trajectory_planner_B.b_i_o < 6;
7505 cartesian_trajectory_planner_B.b_i_o++) {
7506 for (cartesian_trajectory_planner_B.idx = 0;
7507 cartesian_trajectory_planner_B.idx < 6;
7508 cartesian_trajectory_planner_B.idx++) {
7509 cartesian_trajectory_planner_B.s_e = 0.0;
7510 for (cartesian_trajectory_planner_B.i_n = 0;
7511 cartesian_trajectory_planner_B.i_n < 6;
7512 cartesian_trajectory_planner_B.i_n++) {
7513 cartesian_trajectory_planner_B.s_e += unusedU1->data[6 *
7514 cartesian_trajectory_planner_B.i_n +
7515 cartesian_trajectory_planner_B.b_i_o] *
7516 cartesian_trajectory_planner_B.P[6 *
7517 cartesian_trajectory_planner_B.idx +
7518 cartesian_trajectory_planner_B.i_n];
7519 }
7520
7521 cartesian_trajectory_planner_B.idxl = 6 *
7522 cartesian_trajectory_planner_B.idx +
7523 cartesian_trajectory_planner_B.b_i_o;
7524 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
7525 += cartesian_trajectory_planner_B.s_e;
7526 }
7527 }
7528
7529 cartesian_trajectory_planner_B.g_idx_0++;
7530 } else {
7531 guard2 = true;
7532 }
7533 } else {
7534 guard2 = true;
7535 }
7536
7537 if (guard2) {
7538 for (cartesian_trajectory_planner_B.b_i_o = 0;
7539 cartesian_trajectory_planner_B.b_i_o < 6;
7540 cartesian_trajectory_planner_B.b_i_o++) {
7541 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
7542 =
7543 -cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o];
7544 }
7545
7546 cartesian_trajectory_planner_B.idxl = -1;
7547 if (obj->ConstraintsOn) {
7548 cartesian_trajectory_planner_B.b_i_o = x->size[0];
7549 x->size[0] = activeSet->size[0];
7550 car_emxEnsureCapacity_boolean_T(x,
7551 cartesian_trajectory_planner_B.b_i_o);
7552 cartesian_trajectory_planner_B.i_n = activeSet->size[0];
7553 for (cartesian_trajectory_planner_B.b_i_o = 0;
7554 cartesian_trajectory_planner_B.b_i_o <
7555 cartesian_trajectory_planner_B.i_n;
7556 cartesian_trajectory_planner_B.b_i_o++) {
7557 x->data[cartesian_trajectory_planner_B.b_i_o] = !activeSet->
7558 data[cartesian_trajectory_planner_B.b_i_o];
7559 }
7560
7561 if (cartesian_trajectory_planne_any(x)) {
7562 cartesian_trajectory_planner_B.nx_k = activeSet->size[0] - 1;
7563 cartesian_trajectory_planner_B.idx = 0;
7564 for (cartesian_trajectory_planner_B.b_i_o = 0;
7565 cartesian_trajectory_planner_B.b_i_o <=
7566 cartesian_trajectory_planner_B.nx_k;
7567 cartesian_trajectory_planner_B.b_i_o++) {
7568 if (!activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7569 cartesian_trajectory_planner_B.idx++;
7570 }
7571 }
7572
7573 cartesian_trajectory_planner_B.b_i_o = cb->size[0];
7574 cb->size[0] = cartesian_trajectory_planner_B.idx;
7575 carte_emxEnsureCapacity_int32_T(cb,
7576 cartesian_trajectory_planner_B.b_i_o);
7577 cartesian_trajectory_planner_B.idx = 0;
7578 for (cartesian_trajectory_planner_B.b_i_o = 0;
7579 cartesian_trajectory_planner_B.b_i_o <=
7580 cartesian_trajectory_planner_B.nx_k;
7581 cartesian_trajectory_planner_B.b_i_o++) {
7582 if (!activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7583 cb->data[cartesian_trajectory_planner_B.idx] =
7584 cartesian_trajectory_planner_B.b_i_o + 1;
7585 cartesian_trajectory_planner_B.idx++;
7586 }
7587 }
7588
7589 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
7590 alpha->size[0] = cb->size[0];
7591 cartes_emxEnsureCapacity_real_T(alpha,
7592 cartesian_trajectory_planner_B.b_i_o);
7593 cartesian_trajectory_planner_B.i_n = cb->size[0];
7594 for (cartesian_trajectory_planner_B.b_i_o = 0;
7595 cartesian_trajectory_planner_B.b_i_o <
7596 cartesian_trajectory_planner_B.i_n;
7597 cartesian_trajectory_planner_B.b_i_o++) {
7598 alpha->data[cartesian_trajectory_planner_B.b_i_o] =
7599 obj->ConstraintBound->data[cb->
7600 data[cartesian_trajectory_planner_B.b_i_o] - 1];
7601 }
7602
7603 cartesian_trajectory_planner_B.nx_k = activeSet->size[0] - 1;
7604 cartesian_trajectory_planner_B.idx = 0;
7605 for (cartesian_trajectory_planner_B.b_i_o = 0;
7606 cartesian_trajectory_planner_B.b_i_o <=
7607 cartesian_trajectory_planner_B.nx_k;
7608 cartesian_trajectory_planner_B.b_i_o++) {
7609 if (!activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7610 cartesian_trajectory_planner_B.idx++;
7611 }
7612 }
7613
7614 cartesian_trajectory_planner_B.b_i_o = db->size[0];
7615 db->size[0] = cartesian_trajectory_planner_B.idx;
7616 carte_emxEnsureCapacity_int32_T(db,
7617 cartesian_trajectory_planner_B.b_i_o);
7618 cartesian_trajectory_planner_B.idx = 0;
7619 for (cartesian_trajectory_planner_B.b_i_o = 0;
7620 cartesian_trajectory_planner_B.b_i_o <=
7621 cartesian_trajectory_planner_B.nx_k;
7622 cartesian_trajectory_planner_B.b_i_o++) {
7623 if (!activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
7624 db->data[cartesian_trajectory_planner_B.idx] =
7625 cartesian_trajectory_planner_B.b_i_o + 1;
7626 cartesian_trajectory_planner_B.idx++;
7627 }
7628 }
7629
7630 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->
7631 size[0];
7632 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size
7633 [1];
7634 AIn->size[0] = cartesian_trajectory_planner_B.i_n;
7635 AIn->size[1] = db->size[0];
7636 cartes_emxEnsureCapacity_real_T(AIn,
7637 cartesian_trajectory_planner_B.b_i_o);
7638 cartesian_trajectory_planner_B.n_b = db->size[0];
7639 for (cartesian_trajectory_planner_B.b_i_o = 0;
7640 cartesian_trajectory_planner_B.b_i_o <
7641 cartesian_trajectory_planner_B.n_b;
7642 cartesian_trajectory_planner_B.b_i_o++) {
7643 for (cartesian_trajectory_planner_B.idx = 0;
7644 cartesian_trajectory_planner_B.idx <
7645 cartesian_trajectory_planner_B.i_n;
7646 cartesian_trajectory_planner_B.idx++) {
7647 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
7648 cartesian_trajectory_planner_B.b_i_o] =
7649 obj->ConstraintMatrix->data[(db->
7650 data[cartesian_trajectory_planner_B.b_i_o] - 1) *
7651 obj->ConstraintMatrix->size[0] +
7652 cartesian_trajectory_planner_B.idx];
7653 }
7654 }
7655
7656 cartesian_trajectory_planner_B.nx_k = x->size[0];
7657 cartesian_trajectory_planner_B.idx = 0;
7658 cartesian_trajectory_planner_B.b_i_o = ii->size[0];
7659 ii->size[0] = x->size[0];
7660 carte_emxEnsureCapacity_int32_T(ii,
7661 cartesian_trajectory_planner_B.b_i_o);
7662 cartesian_trajectory_planner_B.b_i_o = 1;
7663 exitg3 = false;
7664 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i_o - 1 <=
7665 cartesian_trajectory_planner_B.nx_k - 1)) {
7666 if (x->data[cartesian_trajectory_planner_B.b_i_o - 1]) {
7667 cartesian_trajectory_planner_B.idx++;
7668 ii->data[cartesian_trajectory_planner_B.idx - 1] =
7669 cartesian_trajectory_planner_B.b_i_o;
7670 if (cartesian_trajectory_planner_B.idx >=
7671 cartesian_trajectory_planner_B.nx_k) {
7672 exitg3 = true;
7673 } else {
7674 cartesian_trajectory_planner_B.b_i_o++;
7675 }
7676 } else {
7677 cartesian_trajectory_planner_B.b_i_o++;
7678 }
7679 }
7680
7681 if (x->size[0] == 1) {
7682 if (cartesian_trajectory_planner_B.idx == 0) {
7683 ii->size[0] = 0;
7684 }
7685 } else {
7686 if (1 > cartesian_trajectory_planner_B.idx) {
7687 cartesian_trajectory_planner_B.idx = 0;
7688 }
7689
7690 cartesian_trajectory_planner_B.b_i_o = ii_2->size[0];
7691 ii_2->size[0] = cartesian_trajectory_planner_B.idx;
7692 carte_emxEnsureCapacity_int32_T(ii_2,
7693 cartesian_trajectory_planner_B.b_i_o);
7694 for (cartesian_trajectory_planner_B.b_i_o = 0;
7695 cartesian_trajectory_planner_B.b_i_o <
7696 cartesian_trajectory_planner_B.idx;
7697 cartesian_trajectory_planner_B.b_i_o++) {
7698 ii_2->data[cartesian_trajectory_planner_B.b_i_o] = ii->
7699 data[cartesian_trajectory_planner_B.b_i_o];
7700 }
7701
7702 cartesian_trajectory_planner_B.b_i_o = ii->size[0];
7703 ii->size[0] = ii_2->size[0];
7704 carte_emxEnsureCapacity_int32_T(ii,
7705 cartesian_trajectory_planner_B.b_i_o);
7706 cartesian_trajectory_planner_B.i_n = ii_2->size[0];
7707 for (cartesian_trajectory_planner_B.b_i_o = 0;
7708 cartesian_trajectory_planner_B.b_i_o <
7709 cartesian_trajectory_planner_B.i_n;
7710 cartesian_trajectory_planner_B.b_i_o++) {
7711 ii->data[cartesian_trajectory_planner_B.b_i_o] = ii_2->
7712 data[cartesian_trajectory_planner_B.b_i_o];
7713 }
7714 }
7715
7716 cartesian_trajectory_planner_B.m_hz = AIn->size[1] - 1;
7717 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
7718 cartesian_trajectory_planner_B.b_i_o = L->size[0];
7719 L->size[0] = AIn->size[1];
7720 cartes_emxEnsureCapacity_real_T(L,
7721 cartesian_trajectory_planner_B.b_i_o);
7722 for (cartesian_trajectory_planner_B.b_i_o = 0;
7723 cartesian_trajectory_planner_B.b_i_o <=
7724 cartesian_trajectory_planner_B.m_hz;
7725 cartesian_trajectory_planner_B.b_i_o++) {
7726 L->data[cartesian_trajectory_planner_B.b_i_o] = 0.0;
7727 }
7728
7729 for (cartesian_trajectory_planner_B.nx_k = 0;
7730 cartesian_trajectory_planner_B.nx_k <=
7731 cartesian_trajectory_planner_B.inner;
7732 cartesian_trajectory_planner_B.nx_k++) {
7733 for (cartesian_trajectory_planner_B.j = 0;
7734 cartesian_trajectory_planner_B.j <=
7735 cartesian_trajectory_planner_B.m_hz;
7736 cartesian_trajectory_planner_B.j++) {
7737 L->data[cartesian_trajectory_planner_B.j] += AIn->
7738 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
7739 cartesian_trajectory_planner_B.nx_k] *
7740 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_k];
7741 }
7742 }
7743
7744 cartesian_trajectory_planner_B.m_hz = AIn->size[1] - 1;
7745 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
7746 cartesian_trajectory_planner_B.b_i_o = y->size[0];
7747 y->size[0] = AIn->size[1];
7748 cartes_emxEnsureCapacity_real_T(y,
7749 cartesian_trajectory_planner_B.b_i_o);
7750 for (cartesian_trajectory_planner_B.b_i_o = 0;
7751 cartesian_trajectory_planner_B.b_i_o <=
7752 cartesian_trajectory_planner_B.m_hz;
7753 cartesian_trajectory_planner_B.b_i_o++) {
7754 y->data[cartesian_trajectory_planner_B.b_i_o] = 0.0;
7755 }
7756
7757 for (cartesian_trajectory_planner_B.nx_k = 0;
7758 cartesian_trajectory_planner_B.nx_k <=
7759 cartesian_trajectory_planner_B.inner;
7760 cartesian_trajectory_planner_B.nx_k++) {
7761 for (cartesian_trajectory_planner_B.j = 0;
7762 cartesian_trajectory_planner_B.j <=
7763 cartesian_trajectory_planner_B.m_hz;
7764 cartesian_trajectory_planner_B.j++) {
7765 y->data[cartesian_trajectory_planner_B.j] += AIn->
7766 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
7767 cartesian_trajectory_planner_B.nx_k] *
7768 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.nx_k];
7769 }
7770 }
7771
7772 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
7773 cartes_emxEnsureCapacity_real_T(alpha,
7774 cartesian_trajectory_planner_B.b_i_o);
7775 cartesian_trajectory_planner_B.i_n = alpha->size[0];
7776 for (cartesian_trajectory_planner_B.b_i_o = 0;
7777 cartesian_trajectory_planner_B.b_i_o <
7778 cartesian_trajectory_planner_B.i_n;
7779 cartesian_trajectory_planner_B.b_i_o++) {
7780 alpha->data[cartesian_trajectory_planner_B.b_i_o] =
7781 (alpha->data[cartesian_trajectory_planner_B.b_i_o] - L->
7782 data[cartesian_trajectory_planner_B.b_i_o]) / y->
7783 data[cartesian_trajectory_planner_B.b_i_o];
7784 }
7785
7786 cartesian_trajectory_planner_B.b_i_o = x->size[0];
7787 x->size[0] = alpha->size[0];
7788 car_emxEnsureCapacity_boolean_T(x,
7789 cartesian_trajectory_planner_B.b_i_o);
7790 cartesian_trajectory_planner_B.i_n = alpha->size[0];
7791 for (cartesian_trajectory_planner_B.b_i_o = 0;
7792 cartesian_trajectory_planner_B.b_i_o <
7793 cartesian_trajectory_planner_B.i_n;
7794 cartesian_trajectory_planner_B.b_i_o++) {
7795 x->data[cartesian_trajectory_planner_B.b_i_o] = (alpha->
7796 data[cartesian_trajectory_planner_B.b_i_o] > 0.0);
7797 }
7798
7799 cartesian_trajectory_planner_B.nx_k = x->size[0];
7800 cartesian_trajectory_planner_B.idx = 0;
7801 cartesian_trajectory_planner_B.b_i_o = ii_0->size[0];
7802 ii_0->size[0] = x->size[0];
7803 carte_emxEnsureCapacity_int32_T(ii_0,
7804 cartesian_trajectory_planner_B.b_i_o);
7805 cartesian_trajectory_planner_B.b_i_o = 1;
7806 exitg3 = false;
7807 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i_o - 1 <=
7808 cartesian_trajectory_planner_B.nx_k - 1)) {
7809 if (x->data[cartesian_trajectory_planner_B.b_i_o - 1]) {
7810 cartesian_trajectory_planner_B.idx++;
7811 ii_0->data[cartesian_trajectory_planner_B.idx - 1] =
7812 cartesian_trajectory_planner_B.b_i_o;
7813 if (cartesian_trajectory_planner_B.idx >=
7814 cartesian_trajectory_planner_B.nx_k) {
7815 exitg3 = true;
7816 } else {
7817 cartesian_trajectory_planner_B.b_i_o++;
7818 }
7819 } else {
7820 cartesian_trajectory_planner_B.b_i_o++;
7821 }
7822 }
7823
7824 if (x->size[0] == 1) {
7825 if (cartesian_trajectory_planner_B.idx == 0) {
7826 ii_0->size[0] = 0;
7827 }
7828 } else {
7829 if (1 > cartesian_trajectory_planner_B.idx) {
7830 cartesian_trajectory_planner_B.idx = 0;
7831 }
7832
7833 cartesian_trajectory_planner_B.b_i_o = ii_3->size[0];
7834 ii_3->size[0] = cartesian_trajectory_planner_B.idx;
7835 carte_emxEnsureCapacity_int32_T(ii_3,
7836 cartesian_trajectory_planner_B.b_i_o);
7837 for (cartesian_trajectory_planner_B.b_i_o = 0;
7838 cartesian_trajectory_planner_B.b_i_o <
7839 cartesian_trajectory_planner_B.idx;
7840 cartesian_trajectory_planner_B.b_i_o++) {
7841 ii_3->data[cartesian_trajectory_planner_B.b_i_o] =
7842 ii_0->data[cartesian_trajectory_planner_B.b_i_o];
7843 }
7844
7845 cartesian_trajectory_planner_B.b_i_o = ii_0->size[0];
7846 ii_0->size[0] = ii_3->size[0];
7847 carte_emxEnsureCapacity_int32_T(ii_0,
7848 cartesian_trajectory_planner_B.b_i_o);
7849 cartesian_trajectory_planner_B.i_n = ii_3->size[0];
7850 for (cartesian_trajectory_planner_B.b_i_o = 0;
7851 cartesian_trajectory_planner_B.b_i_o <
7852 cartesian_trajectory_planner_B.i_n;
7853 cartesian_trajectory_planner_B.b_i_o++) {
7854 ii_0->data[cartesian_trajectory_planner_B.b_i_o] =
7855 ii_3->data[cartesian_trajectory_planner_B.b_i_o];
7856 }
7857 }
7858
7859 cartesian_trajectory_planner_B.b_i_o = L->size[0];
7860 L->size[0] = ii_0->size[0];
7861 cartes_emxEnsureCapacity_real_T(L,
7862 cartesian_trajectory_planner_B.b_i_o);
7863 cartesian_trajectory_planner_B.i_n = ii_0->size[0];
7864 for (cartesian_trajectory_planner_B.b_i_o = 0;
7865 cartesian_trajectory_planner_B.b_i_o <
7866 cartesian_trajectory_planner_B.i_n;
7867 cartesian_trajectory_planner_B.b_i_o++) {
7868 L->data[cartesian_trajectory_planner_B.b_i_o] = ii_0->
7869 data[cartesian_trajectory_planner_B.b_i_o];
7870 }
7871
7872 if (L->size[0] != 0) {
7873 cartesian_trajectory_planner_B.nx_k = alpha->size[0] - 1;
7874 cartesian_trajectory_planner_B.idx = 0;
7875 for (cartesian_trajectory_planner_B.b_i_o = 0;
7876 cartesian_trajectory_planner_B.b_i_o <=
7877 cartesian_trajectory_planner_B.nx_k;
7878 cartesian_trajectory_planner_B.b_i_o++) {
7879 if (alpha->data[cartesian_trajectory_planner_B.b_i_o] > 0.0)
7880 {
7881 cartesian_trajectory_planner_B.idx++;
7882 }
7883 }
7884
7885 cartesian_trajectory_planner_B.b_i_o = fb->size[0];
7886 fb->size[0] = cartesian_trajectory_planner_B.idx;
7887 carte_emxEnsureCapacity_int32_T(fb,
7888 cartesian_trajectory_planner_B.b_i_o);
7889 cartesian_trajectory_planner_B.idx = 0;
7890 for (cartesian_trajectory_planner_B.b_i_o = 0;
7891 cartesian_trajectory_planner_B.b_i_o <=
7892 cartesian_trajectory_planner_B.nx_k;
7893 cartesian_trajectory_planner_B.b_i_o++) {
7894 if (alpha->data[cartesian_trajectory_planner_B.b_i_o] > 0.0)
7895 {
7896 fb->data[cartesian_trajectory_planner_B.idx] =
7897 cartesian_trajectory_planner_B.b_i_o + 1;
7898 cartesian_trajectory_planner_B.idx++;
7899 }
7900 }
7901
7902 cartesian_trajectory_planner_B.n_b = fb->size[0];
7903 if (fb->size[0] <= 2) {
7904 if (fb->size[0] == 1) {
7905 cartesian_trajectory_planner_B.s_e = alpha->data[fb->data
7906 [0] - 1];
7907 cartesian_trajectory_planner_B.idxl = 0;
7908 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
7909 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
7910 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
7911 [fb->data[1] - 1])))) {
7912 cartesian_trajectory_planner_B.s_e = alpha->data[fb->data
7913 [1] - 1];
7914 cartesian_trajectory_planner_B.idxl = 1;
7915 } else {
7916 cartesian_trajectory_planner_B.s_e = alpha->data[fb->data
7917 [0] - 1];
7918 cartesian_trajectory_planner_B.idxl = 0;
7919 }
7920 } else {
7921 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
7922 cartesian_trajectory_planner_B.idxl = 1;
7923 } else {
7924 cartesian_trajectory_planner_B.idxl = 0;
7925 cartesian_trajectory_planner_B.b_i_o = 2;
7926 exitg3 = false;
7927 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i_o <=
7928 fb->size[0])) {
7929 if (!rtIsNaN(alpha->data[fb->
7930 data[cartesian_trajectory_planner_B.b_i_o -
7931 1] - 1])) {
7932 cartesian_trajectory_planner_B.idxl =
7933 cartesian_trajectory_planner_B.b_i_o;
7934 exitg3 = true;
7935 } else {
7936 cartesian_trajectory_planner_B.b_i_o++;
7937 }
7938 }
7939 }
7940
7941 if (cartesian_trajectory_planner_B.idxl == 0) {
7942 cartesian_trajectory_planner_B.s_e = alpha->data[fb->data
7943 [0] - 1];
7944 } else {
7945 cartesian_trajectory_planner_B.s_e = alpha->data[fb->
7946 data[cartesian_trajectory_planner_B.idxl - 1] - 1];
7947 cartesian_trajectory_planner_B.nx_k =
7948 cartesian_trajectory_planner_B.idxl;
7949 for (cartesian_trajectory_planner_B.b_i_o =
7950 cartesian_trajectory_planner_B.idxl + 1;
7951 cartesian_trajectory_planner_B.b_i_o <=
7952 cartesian_trajectory_planner_B.n_b;
7953 cartesian_trajectory_planner_B.b_i_o++) {
7954 if (cartesian_trajectory_planner_B.s_e > alpha->data
7955 [fb->data[cartesian_trajectory_planner_B.b_i_o - 1]
7956 - 1]) {
7957 cartesian_trajectory_planner_B.s_e = alpha->data
7958 [fb->data[cartesian_trajectory_planner_B.b_i_o - 1]
7959 - 1];
7960 cartesian_trajectory_planner_B.nx_k =
7961 cartesian_trajectory_planner_B.b_i_o;
7962 }
7963 }
7964
7965 cartesian_trajectory_planner_B.idxl =
7966 cartesian_trajectory_planner_B.nx_k - 1;
7967 }
7968 }
7969
7970 cartesian_trajectory_planner_B.idxl = ii->data
7971 [static_cast<int32_T>(L->
7972 data[cartesian_trajectory_planner_B.idxl]) - 1];
7973 } else {
7974 cartesian_trajectory_planner_B.s_e = 0.0;
7975 }
7976 } else {
7977 cartesian_trajectory_planner_B.s_e = 0.0;
7978 }
7979 } else {
7980 cartesian_trajectory_planner_B.s_e = 0.0;
7981 }
7982
7983 if (cartesian_trajectory_planner_B.s_e > 0.0) {
7984 if (1.0 < cartesian_trajectory_planner_B.s_e) {
7985 cartesian_trajectory_planner_B.b_gamma = 1.0;
7986 } else {
7987 cartesian_trajectory_planner_B.b_gamma =
7988 cartesian_trajectory_planner_B.s_e;
7989 }
7990 } else {
7991 cartesian_trajectory_planner_B.b_gamma = 1.0;
7992 }
7993
7994 cartesian_trajectory_planner_B.beta = obj->ArmijoRuleBeta;
7995 cartesian_trajectory_planner_B.sigma = obj->ArmijoRuleSigma;
7996 for (cartesian_trajectory_planner_B.b_i_o = 0;
7997 cartesian_trajectory_planner_B.b_i_o < 6;
7998 cartesian_trajectory_planner_B.b_i_o++) {
7999 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8000 = cartesian_trajectory_planner_B.b_gamma *
8001 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
8002 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8003 }
8004
8005 cartesian_IKHelpers_computeCost
8006 (cartesian_trajectory_planner_B.sNew_a, obj->ExtraArgs,
8007 &cartesian_trajectory_planner_B.costNew,
8008 cartesian_trajectory_planner_B.V, unusedU1, &c);
8009 obj->ExtraArgs = c;
8010 cartesian_trajectory_planner_B.m = 0.0;
8011 do {
8012 exitg1 = 0;
8013 for (cartesian_trajectory_planner_B.i_n = 0;
8014 cartesian_trajectory_planner_B.i_n < 6;
8015 cartesian_trajectory_planner_B.i_n++) {
8016 xSol[cartesian_trajectory_planner_B.i_n] =
8017 cartesian_trajectory_planner_B.b_gamma *
8018 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.i_n];
8019 }
8020
8021 cartesian_trajectory_planner_B.b_i_o = sigma->size[0] *
8022 sigma->size[1];
8023 sigma->size[0] = 1;
8024 sigma->size[1] = grad->size[0];
8025 cartes_emxEnsureCapacity_real_T(sigma,
8026 cartesian_trajectory_planner_B.b_i_o);
8027 cartesian_trajectory_planner_B.i_n = grad->size[0];
8028 for (cartesian_trajectory_planner_B.b_i_o = 0;
8029 cartesian_trajectory_planner_B.b_i_o <
8030 cartesian_trajectory_planner_B.i_n;
8031 cartesian_trajectory_planner_B.b_i_o++) {
8032 sigma->data[cartesian_trajectory_planner_B.b_i_o] =
8033 -cartesian_trajectory_planner_B.sigma * grad->
8034 data[cartesian_trajectory_planner_B.b_i_o];
8035 }
8036
8037 cartesian_trajectory_planner_B.sigma_i = 0.0;
8038 for (cartesian_trajectory_planner_B.b_i_o = 0;
8039 cartesian_trajectory_planner_B.b_i_o < 6;
8040 cartesian_trajectory_planner_B.b_i_o++) {
8041 cartesian_trajectory_planner_B.sigma_i += sigma->
8042 data[cartesian_trajectory_planner_B.b_i_o] *
8043 xSol[cartesian_trajectory_planner_B.b_i_o];
8044 }
8045
8046 if (cartesian_trajectory_planner_B.cost -
8047 cartesian_trajectory_planner_B.costNew <
8048 cartesian_trajectory_planner_B.sigma_i) {
8049 cartesian_trajectory_planner_B.flag =
8050 (cartesian_trajectory_planner_B.b_gamma < obj->StepTolerance);
8051 if (cartesian_trajectory_planner_B.flag) {
8052 for (cartesian_trajectory_planner_B.i_n = 0;
8053 cartesian_trajectory_planner_B.i_n < 6;
8054 cartesian_trajectory_planner_B.i_n++) {
8055 xSol[cartesian_trajectory_planner_B.i_n] =
8056 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_n];
8057 }
8058
8059 *exitFlag = StepSizeBelowMinimum;
8060 args = obj->ExtraArgs;
8061 for (cartesian_trajectory_planner_B.b_i_o = 0;
8062 cartesian_trajectory_planner_B.b_i_o < 36;
8063 cartesian_trajectory_planner_B.b_i_o++) {
8064 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
8065 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
8066 }
8067
8068 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8069 grad->size[0] = args->ErrTemp->size[0];
8070 cartes_emxEnsureCapacity_real_T(grad,
8071 cartesian_trajectory_planner_B.b_i_o);
8072 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
8073 for (cartesian_trajectory_planner_B.b_i_o = 0;
8074 cartesian_trajectory_planner_B.b_i_o <
8075 cartesian_trajectory_planner_B.i_n;
8076 cartesian_trajectory_planner_B.b_i_o++) {
8077 grad->data[cartesian_trajectory_planner_B.b_i_o] =
8078 args->ErrTemp->data[cartesian_trajectory_planner_B.b_i_o];
8079 }
8080
8081 for (cartesian_trajectory_planner_B.b_i_o = 0;
8082 cartesian_trajectory_planner_B.b_i_o < 6;
8083 cartesian_trajectory_planner_B.b_i_o++) {
8084 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8085 = 0.0;
8086 for (cartesian_trajectory_planner_B.idx = 0;
8087 cartesian_trajectory_planner_B.idx < 6;
8088 cartesian_trajectory_planner_B.idx++) {
8089 cartesian_trajectory_planner_B.A_d =
8090 cartesian_trajectory_planner_B.unusedU0[6 *
8091 cartesian_trajectory_planner_B.idx +
8092 cartesian_trajectory_planner_B.b_i_o] * grad->
8093 data[cartesian_trajectory_planner_B.idx] +
8094 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8095 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8096 = cartesian_trajectory_planner_B.A_d;
8097 }
8098 }
8099
8100 *err = cartesian_trajectory_pla_norm_a
8101 (cartesian_trajectory_planner_B.x);
8102 *iter = static_cast<real_T>
8103 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
8104 exitg1 = 1;
8105 } else {
8106 cartesian_trajectory_planner_B.b_gamma *=
8107 cartesian_trajectory_planner_B.beta;
8108 cartesian_trajectory_planner_B.m++;
8109 for (cartesian_trajectory_planner_B.b_i_o = 0;
8110 cartesian_trajectory_planner_B.b_i_o < 6;
8111 cartesian_trajectory_planner_B.b_i_o++) {
8112 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8113 = cartesian_trajectory_planner_B.b_gamma *
8114 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
8115 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8116 }
8117
8118 cartesian_IKHelpers_computeCost
8119 (cartesian_trajectory_planner_B.sNew_a, obj->ExtraArgs,
8120 &cartesian_trajectory_planner_B.costNew,
8121 cartesian_trajectory_planner_B.V, unusedU1, &d);
8122 obj->ExtraArgs = d;
8123 }
8124 } else {
8125 for (cartesian_trajectory_planner_B.b_i_o = 0;
8126 cartesian_trajectory_planner_B.b_i_o < 6;
8127 cartesian_trajectory_planner_B.b_i_o++) {
8128 xSol[cartesian_trajectory_planner_B.b_i_o] +=
8129 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8130 }
8131
8132 args = obj->ExtraArgs;
8133 cartesian_trajectory_planner_B.b_i_o = alpha->size[0];
8134 alpha->size[0] = args->GradTemp->size[0];
8135 cartes_emxEnsureCapacity_real_T(alpha,
8136 cartesian_trajectory_planner_B.b_i_o);
8137 cartesian_trajectory_planner_B.i_n = args->GradTemp->size[0];
8138 for (cartesian_trajectory_planner_B.b_i_o = 0;
8139 cartesian_trajectory_planner_B.b_i_o <
8140 cartesian_trajectory_planner_B.i_n;
8141 cartesian_trajectory_planner_B.b_i_o++) {
8142 alpha->data[cartesian_trajectory_planner_B.b_i_o] =
8143 args->GradTemp->data[cartesian_trajectory_planner_B.b_i_o];
8144 }
8145
8146 exitg1 = 2;
8147 }
8148 } while (exitg1 == 0);
8149
8150 if (exitg1 == 1) {
8151 exitg2 = 1;
8152 } else if ((cartesian_trajectory_planner_B.m == 0.0) && (fabs
8153 (cartesian_trajectory_planner_B.b_gamma -
8154 cartesian_trajectory_planner_B.s_e) <
8155 1.4901161193847656E-8)) {
8156 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0];
8157 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8158 grad->size[0] = cartesian_trajectory_planner_B.i_n;
8159 cartes_emxEnsureCapacity_real_T(grad,
8160 cartesian_trajectory_planner_B.b_i_o);
8161 for (cartesian_trajectory_planner_B.b_i_o = 0;
8162 cartesian_trajectory_planner_B.b_i_o <
8163 cartesian_trajectory_planner_B.i_n;
8164 cartesian_trajectory_planner_B.b_i_o++) {
8165 grad->data[cartesian_trajectory_planner_B.b_i_o] =
8166 obj->ConstraintMatrix->data
8167 [(cartesian_trajectory_planner_B.idxl - 1) *
8168 obj->ConstraintMatrix->size[0] +
8169 cartesian_trajectory_planner_B.b_i_o];
8170 }
8171
8172 activeSet->data[cartesian_trajectory_planner_B.idxl - 1] = true;
8173 cartesian_trajectory_planner_B.nx_k = activeSet->size[0] - 1;
8174 cartesian_trajectory_planner_B.idx = 0;
8175 for (cartesian_trajectory_planner_B.b_i_o = 0;
8176 cartesian_trajectory_planner_B.b_i_o <=
8177 cartesian_trajectory_planner_B.nx_k;
8178 cartesian_trajectory_planner_B.b_i_o++) {
8179 if (activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
8180 cartesian_trajectory_planner_B.idx++;
8181 }
8182 }
8183
8184 cartesian_trajectory_planner_B.b_i_o = gb->size[0];
8185 gb->size[0] = cartesian_trajectory_planner_B.idx;
8186 carte_emxEnsureCapacity_int32_T(gb,
8187 cartesian_trajectory_planner_B.b_i_o);
8188 cartesian_trajectory_planner_B.idx = 0;
8189 for (cartesian_trajectory_planner_B.b_i_o = 0;
8190 cartesian_trajectory_planner_B.b_i_o <=
8191 cartesian_trajectory_planner_B.nx_k;
8192 cartesian_trajectory_planner_B.b_i_o++) {
8193 if (activeSet->data[cartesian_trajectory_planner_B.b_i_o]) {
8194 gb->data[cartesian_trajectory_planner_B.idx] =
8195 cartesian_trajectory_planner_B.b_i_o + 1;
8196 cartesian_trajectory_planner_B.idx++;
8197 }
8198 }
8199
8200 cartesian_trajectory_planner_B.i_n = obj->ConstraintMatrix->size[0];
8201 cartesian_trajectory_planner_B.b_i_o = A->size[0] * A->size[1];
8202 A->size[0] = cartesian_trajectory_planner_B.i_n;
8203 A->size[1] = gb->size[0];
8204 cartes_emxEnsureCapacity_real_T(A,
8205 cartesian_trajectory_planner_B.b_i_o);
8206 cartesian_trajectory_planner_B.n_b = gb->size[0];
8207 for (cartesian_trajectory_planner_B.b_i_o = 0;
8208 cartesian_trajectory_planner_B.b_i_o <
8209 cartesian_trajectory_planner_B.n_b;
8210 cartesian_trajectory_planner_B.b_i_o++) {
8211 for (cartesian_trajectory_planner_B.idx = 0;
8212 cartesian_trajectory_planner_B.idx <
8213 cartesian_trajectory_planner_B.i_n;
8214 cartesian_trajectory_planner_B.idx++) {
8215 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
8216 cartesian_trajectory_planner_B.b_i_o] =
8217 obj->ConstraintMatrix->data[(gb->
8218 data[cartesian_trajectory_planner_B.b_i_o] - 1) *
8219 obj->ConstraintMatrix->size[0] +
8220 cartesian_trajectory_planner_B.idx];
8221 }
8222 }
8223
8224 cartesian_trajectory_planner_B.b_i_o = AIn->size[0] * AIn->size[1];
8225 AIn->size[0] = grad->size[0];
8226 AIn->size[1] = grad->size[0];
8227 cartes_emxEnsureCapacity_real_T(AIn,
8228 cartesian_trajectory_planner_B.b_i_o);
8229 cartesian_trajectory_planner_B.i_n = grad->size[0];
8230 for (cartesian_trajectory_planner_B.b_i_o = 0;
8231 cartesian_trajectory_planner_B.b_i_o <
8232 cartesian_trajectory_planner_B.i_n;
8233 cartesian_trajectory_planner_B.b_i_o++) {
8234 cartesian_trajectory_planner_B.n_b = grad->size[0];
8235 for (cartesian_trajectory_planner_B.idx = 0;
8236 cartesian_trajectory_planner_B.idx <
8237 cartesian_trajectory_planner_B.n_b;
8238 cartesian_trajectory_planner_B.idx++) {
8239 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
8240 cartesian_trajectory_planner_B.b_i_o] = grad->
8241 data[cartesian_trajectory_planner_B.idx] * grad->
8242 data[cartesian_trajectory_planner_B.b_i_o];
8243 }
8244 }
8245
8246 cartesian_trajectory_planner_B.m_hz = AIn->size[0] - 1;
8247 cartesian_trajectory_planner_B.inner = AIn->size[1] - 1;
8248 cartesian_trajectory_planner_B.b_i_o = y_0->size[0] * y_0->size[1];
8249 y_0->size[0] = AIn->size[0];
8250 y_0->size[1] = 6;
8251 cartes_emxEnsureCapacity_real_T(y_0,
8252 cartesian_trajectory_planner_B.b_i_o);
8253 for (cartesian_trajectory_planner_B.idx = 0;
8254 cartesian_trajectory_planner_B.idx < 6;
8255 cartesian_trajectory_planner_B.idx++) {
8256 cartesian_trajectory_planner_B.coffset =
8257 (cartesian_trajectory_planner_B.m_hz + 1) *
8258 cartesian_trajectory_planner_B.idx - 1;
8259 cartesian_trajectory_planner_B.boffset =
8260 cartesian_trajectory_planner_B.idx * 6 - 1;
8261 for (cartesian_trajectory_planner_B.b_i_o = 0;
8262 cartesian_trajectory_planner_B.b_i_o <=
8263 cartesian_trajectory_planner_B.m_hz;
8264 cartesian_trajectory_planner_B.b_i_o++) {
8265 y_0->data[(cartesian_trajectory_planner_B.coffset +
8266 cartesian_trajectory_planner_B.b_i_o) + 1] = 0.0;
8267 }
8268
8269 for (cartesian_trajectory_planner_B.nx_k = 0;
8270 cartesian_trajectory_planner_B.nx_k <=
8271 cartesian_trajectory_planner_B.inner;
8272 cartesian_trajectory_planner_B.nx_k++) {
8273 cartesian_trajectory_planner_B.aoffset =
8274 cartesian_trajectory_planner_B.nx_k * AIn->size[0] - 1;
8275 cartesian_trajectory_planner_B.s_e =
8276 cartesian_trajectory_planner_B.H
8277 [(cartesian_trajectory_planner_B.boffset +
8278 cartesian_trajectory_planner_B.nx_k) + 1];
8279 for (cartesian_trajectory_planner_B.j = 0;
8280 cartesian_trajectory_planner_B.j <=
8281 cartesian_trajectory_planner_B.m_hz;
8282 cartesian_trajectory_planner_B.j++) {
8283 cartesian_trajectory_planner_B.i_n =
8284 cartesian_trajectory_planner_B.j + 1;
8285 cartesian_trajectory_planner_B.b_i_o =
8286 cartesian_trajectory_planner_B.coffset +
8287 cartesian_trajectory_planner_B.i_n;
8288 y_0->data[cartesian_trajectory_planner_B.b_i_o] += AIn->
8289 data[cartesian_trajectory_planner_B.aoffset +
8290 cartesian_trajectory_planner_B.i_n] *
8291 cartesian_trajectory_planner_B.s_e;
8292 }
8293 }
8294 }
8295
8296 cartesian_trajectory_planner_B.b_i_o = grad_1->size[0] *
8297 grad_1->size[1];
8298 grad_1->size[0] = 1;
8299 grad_1->size[1] = grad->size[0];
8300 cartes_emxEnsureCapacity_real_T(grad_1,
8301 cartesian_trajectory_planner_B.b_i_o);
8302 cartesian_trajectory_planner_B.i_n = grad->size[0];
8303 for (cartesian_trajectory_planner_B.b_i_o = 0;
8304 cartesian_trajectory_planner_B.b_i_o <
8305 cartesian_trajectory_planner_B.i_n;
8306 cartesian_trajectory_planner_B.b_i_o++) {
8307 grad_1->data[cartesian_trajectory_planner_B.b_i_o] = grad->
8308 data[cartesian_trajectory_planner_B.b_i_o];
8309 }
8310
8311 cartesian_trajectory_planner_B.beta = 0.0;
8312 for (cartesian_trajectory_planner_B.b_i_o = 0;
8313 cartesian_trajectory_planner_B.b_i_o < 6;
8314 cartesian_trajectory_planner_B.b_i_o++) {
8315 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8316 = 0.0;
8317 for (cartesian_trajectory_planner_B.idx = 0;
8318 cartesian_trajectory_planner_B.idx < 6;
8319 cartesian_trajectory_planner_B.idx++) {
8320 cartesian_trajectory_planner_B.sigma =
8321 cartesian_trajectory_planner_B.H[6 *
8322 cartesian_trajectory_planner_B.b_i_o +
8323 cartesian_trajectory_planner_B.idx] * grad_1->
8324 data[cartesian_trajectory_planner_B.idx] +
8325 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o];
8326 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8327 = cartesian_trajectory_planner_B.sigma;
8328 }
8329
8330 cartesian_trajectory_planner_B.beta +=
8331 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8332 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8333 }
8334
8335 cartesian_trajectory_planner_B.s_e = 1.0 /
8336 cartesian_trajectory_planner_B.beta;
8337 for (cartesian_trajectory_planner_B.b_i_o = 0;
8338 cartesian_trajectory_planner_B.b_i_o < 6;
8339 cartesian_trajectory_planner_B.b_i_o++) {
8340 for (cartesian_trajectory_planner_B.idx = 0;
8341 cartesian_trajectory_planner_B.idx < 6;
8342 cartesian_trajectory_planner_B.idx++) {
8343 cartesian_trajectory_planner_B.idxl =
8344 cartesian_trajectory_planner_B.b_i_o + 6 *
8345 cartesian_trajectory_planner_B.idx;
8346 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
8347 = 0.0;
8348 for (cartesian_trajectory_planner_B.i_n = 0;
8349 cartesian_trajectory_planner_B.i_n < 6;
8350 cartesian_trajectory_planner_B.i_n++) {
8351 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
8352 += cartesian_trajectory_planner_B.H[6 *
8353 cartesian_trajectory_planner_B.i_n +
8354 cartesian_trajectory_planner_B.b_i_o] * y_0->data[6 *
8355 cartesian_trajectory_planner_B.idx +
8356 cartesian_trajectory_planner_B.i_n];
8357 }
8358 }
8359 }
8360
8361 for (cartesian_trajectory_planner_B.b_i_o = 0;
8362 cartesian_trajectory_planner_B.b_i_o < 36;
8363 cartesian_trajectory_planner_B.b_i_o++) {
8364 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i_o]
8365 -= cartesian_trajectory_planner_B.s_e *
8366 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i_o];
8367 }
8368
8369 guard1 = true;
8370 } else {
8371 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8372 grad->size[0] = alpha->size[0];
8373 cartes_emxEnsureCapacity_real_T(grad,
8374 cartesian_trajectory_planner_B.b_i_o);
8375 cartesian_trajectory_planner_B.i_n = alpha->size[0];
8376 for (cartesian_trajectory_planner_B.b_i_o = 0;
8377 cartesian_trajectory_planner_B.b_i_o <
8378 cartesian_trajectory_planner_B.i_n;
8379 cartesian_trajectory_planner_B.b_i_o++) {
8380 grad->data[cartesian_trajectory_planner_B.b_i_o] = alpha->
8381 data[cartesian_trajectory_planner_B.b_i_o] - grad->
8382 data[cartesian_trajectory_planner_B.b_i_o];
8383 }
8384
8385 cartesian_trajectory_planner_B.b_gamma = 0.0;
8386 for (cartesian_trajectory_planner_B.b_i_o = 0;
8387 cartesian_trajectory_planner_B.b_i_o < 6;
8388 cartesian_trajectory_planner_B.b_i_o++) {
8389 cartesian_trajectory_planner_B.b_gamma +=
8390 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
8391 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8392 }
8393
8394 cartesian_trajectory_planner_B.b_i_o = tmp->size[0] * tmp->size[1];
8395 tmp->size[0] = 1;
8396 tmp->size[1] = grad->size[0];
8397 cartes_emxEnsureCapacity_real_T(tmp,
8398 cartesian_trajectory_planner_B.b_i_o);
8399 cartesian_trajectory_planner_B.i_n = grad->size[0];
8400 for (cartesian_trajectory_planner_B.b_i_o = 0;
8401 cartesian_trajectory_planner_B.b_i_o <
8402 cartesian_trajectory_planner_B.i_n;
8403 cartesian_trajectory_planner_B.b_i_o++) {
8404 tmp->data[cartesian_trajectory_planner_B.b_i_o] = 0.2 *
8405 grad->data[cartesian_trajectory_planner_B.b_i_o];
8406 }
8407
8408 cartesian_trajectory_planner_B.s_e = 0.0;
8409 for (cartesian_trajectory_planner_B.b_i_o = 0;
8410 cartesian_trajectory_planner_B.b_i_o < 6;
8411 cartesian_trajectory_planner_B.b_i_o++) {
8412 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8413 = 0.0;
8414 for (cartesian_trajectory_planner_B.idx = 0;
8415 cartesian_trajectory_planner_B.idx < 6;
8416 cartesian_trajectory_planner_B.idx++) {
8417 cartesian_trajectory_planner_B.beta =
8418 cartesian_trajectory_planner_B.H[6 *
8419 cartesian_trajectory_planner_B.b_i_o +
8420 cartesian_trajectory_planner_B.idx] * tmp->
8421 data[cartesian_trajectory_planner_B.idx] +
8422 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o];
8423 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8424 = cartesian_trajectory_planner_B.beta;
8425 }
8426
8427 cartesian_trajectory_planner_B.s_e +=
8428 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8429 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8430 }
8431
8432 if (cartesian_trajectory_planner_B.b_gamma <
8433 cartesian_trajectory_planner_B.s_e) {
8434 cartesian_trajectory_planner_B.b_i_o = tmp_0->size[0] *
8435 tmp_0->size[1];
8436 tmp_0->size[0] = 1;
8437 tmp_0->size[1] = grad->size[0];
8438 cartes_emxEnsureCapacity_real_T(tmp_0,
8439 cartesian_trajectory_planner_B.b_i_o);
8440 cartesian_trajectory_planner_B.i_n = grad->size[0];
8441 for (cartesian_trajectory_planner_B.b_i_o = 0;
8442 cartesian_trajectory_planner_B.b_i_o <
8443 cartesian_trajectory_planner_B.i_n;
8444 cartesian_trajectory_planner_B.b_i_o++) {
8445 tmp_0->data[cartesian_trajectory_planner_B.b_i_o] = 0.8 *
8446 grad->data[cartesian_trajectory_planner_B.b_i_o];
8447 }
8448
8449 cartesian_trajectory_planner_B.s_e = 0.0;
8450 for (cartesian_trajectory_planner_B.b_i_o = 0;
8451 cartesian_trajectory_planner_B.b_i_o < 6;
8452 cartesian_trajectory_planner_B.b_i_o++) {
8453 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8454 = 0.0;
8455 for (cartesian_trajectory_planner_B.idx = 0;
8456 cartesian_trajectory_planner_B.idx < 6;
8457 cartesian_trajectory_planner_B.idx++) {
8458 cartesian_trajectory_planner_B.beta =
8459 cartesian_trajectory_planner_B.H[6 *
8460 cartesian_trajectory_planner_B.b_i_o +
8461 cartesian_trajectory_planner_B.idx] * tmp_0->
8462 data[cartesian_trajectory_planner_B.idx] +
8463 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o];
8464 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8465 = cartesian_trajectory_planner_B.beta;
8466 }
8467
8468 cartesian_trajectory_planner_B.s_e +=
8469 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8470 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8471 }
8472
8473 cartesian_trajectory_planner_B.b_i_o = grad_0->size[0] *
8474 grad_0->size[1];
8475 grad_0->size[0] = 1;
8476 grad_0->size[1] = grad->size[0];
8477 cartes_emxEnsureCapacity_real_T(grad_0,
8478 cartesian_trajectory_planner_B.b_i_o);
8479 cartesian_trajectory_planner_B.i_n = grad->size[0];
8480 for (cartesian_trajectory_planner_B.b_i_o = 0;
8481 cartesian_trajectory_planner_B.b_i_o <
8482 cartesian_trajectory_planner_B.i_n;
8483 cartesian_trajectory_planner_B.b_i_o++) {
8484 grad_0->data[cartesian_trajectory_planner_B.b_i_o] =
8485 grad->data[cartesian_trajectory_planner_B.b_i_o];
8486 }
8487
8488 cartesian_trajectory_planner_B.beta = 0.0;
8489 cartesian_trajectory_planner_B.b_gamma = 0.0;
8490 for (cartesian_trajectory_planner_B.b_i_o = 0;
8491 cartesian_trajectory_planner_B.b_i_o < 6;
8492 cartesian_trajectory_planner_B.b_i_o++) {
8493 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8494 = 0.0;
8495 for (cartesian_trajectory_planner_B.idx = 0;
8496 cartesian_trajectory_planner_B.idx < 6;
8497 cartesian_trajectory_planner_B.idx++) {
8498 cartesian_trajectory_planner_B.sigma =
8499 cartesian_trajectory_planner_B.H[6 *
8500 cartesian_trajectory_planner_B.b_i_o +
8501 cartesian_trajectory_planner_B.idx] * grad_0->
8502 data[cartesian_trajectory_planner_B.idx] +
8503 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o];
8504 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8505 = cartesian_trajectory_planner_B.sigma;
8506 }
8507
8508 cartesian_trajectory_planner_B.beta +=
8509 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8510 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8511 cartesian_trajectory_planner_B.b_gamma +=
8512 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o]
8513 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8514 }
8515
8516 cartesian_trajectory_planner_B.b_gamma =
8517 cartesian_trajectory_planner_B.s_e /
8518 (cartesian_trajectory_planner_B.beta -
8519 cartesian_trajectory_planner_B.b_gamma);
8520 } else {
8521 cartesian_trajectory_planner_B.b_gamma = 1.0;
8522 }
8523
8524 cartesian_trajectory_planner_B.beta = 0.0;
8525 for (cartesian_trajectory_planner_B.b_i_o = 0;
8526 cartesian_trajectory_planner_B.b_i_o < 6;
8527 cartesian_trajectory_planner_B.b_i_o++) {
8528 cartesian_trajectory_planner_B.s_e = 0.0;
8529 for (cartesian_trajectory_planner_B.idx = 0;
8530 cartesian_trajectory_planner_B.idx < 6;
8531 cartesian_trajectory_planner_B.idx++) {
8532 cartesian_trajectory_planner_B.s_e +=
8533 cartesian_trajectory_planner_B.H[6 *
8534 cartesian_trajectory_planner_B.idx +
8535 cartesian_trajectory_planner_B.b_i_o] * (1.0 -
8536 cartesian_trajectory_planner_B.b_gamma) * grad->
8537 data[cartesian_trajectory_planner_B.idx];
8538 }
8539
8540 cartesian_trajectory_planner_B.s_e +=
8541 cartesian_trajectory_planner_B.b_gamma *
8542 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i_o];
8543 cartesian_trajectory_planner_B.beta +=
8544 cartesian_trajectory_planner_B.s_e * grad->
8545 data[cartesian_trajectory_planner_B.b_i_o];
8546 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8547 = cartesian_trajectory_planner_B.s_e;
8548 }
8549
8550 cartesian_trajectory_planner_B.b_i_o = sNew->size[0] * sNew->size
8551 [1];
8552 sNew->size[0] = 6;
8553 sNew->size[1] = grad->size[0];
8554 cartes_emxEnsureCapacity_real_T(sNew,
8555 cartesian_trajectory_planner_B.b_i_o);
8556 cartesian_trajectory_planner_B.i_n = grad->size[0];
8557 for (cartesian_trajectory_planner_B.b_i_o = 0;
8558 cartesian_trajectory_planner_B.b_i_o <
8559 cartesian_trajectory_planner_B.i_n;
8560 cartesian_trajectory_planner_B.b_i_o++) {
8561 for (cartesian_trajectory_planner_B.idx = 0;
8562 cartesian_trajectory_planner_B.idx < 6;
8563 cartesian_trajectory_planner_B.idx++) {
8564 cartesian_trajectory_planner_B.s_e =
8565 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.idx]
8566 * grad->data[cartesian_trajectory_planner_B.b_i_o];
8567 sNew->data[cartesian_trajectory_planner_B.idx + 6 *
8568 cartesian_trajectory_planner_B.b_i_o] =
8569 cartesian_trajectory_planner_B.s_e /
8570 cartesian_trajectory_planner_B.beta;
8571 }
8572 }
8573
8574 for (cartesian_trajectory_planner_B.b_i_o = 0;
8575 cartesian_trajectory_planner_B.b_i_o < 36;
8576 cartesian_trajectory_planner_B.b_i_o++) {
8577 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i_o]
8578 =
8579 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
8580 - sNew->data[cartesian_trajectory_planner_B.b_i_o];
8581 }
8582
8583 for (cartesian_trajectory_planner_B.b_i_o = 0;
8584 cartesian_trajectory_planner_B.b_i_o < 6;
8585 cartesian_trajectory_planner_B.b_i_o++) {
8586 for (cartesian_trajectory_planner_B.idx = 0;
8587 cartesian_trajectory_planner_B.idx < 6;
8588 cartesian_trajectory_planner_B.idx++) {
8589 cartesian_trajectory_planner_B.nx_k =
8590 cartesian_trajectory_planner_B.b_i_o + 6 *
8591 cartesian_trajectory_planner_B.idx;
8592 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_k]
8593 = 0.0;
8594 for (cartesian_trajectory_planner_B.i_n = 0;
8595 cartesian_trajectory_planner_B.i_n < 6;
8596 cartesian_trajectory_planner_B.i_n++) {
8597 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_k]
8598 += cartesian_trajectory_planner_B.V[6 *
8599 cartesian_trajectory_planner_B.i_n +
8600 cartesian_trajectory_planner_B.b_i_o] *
8601 cartesian_trajectory_planner_B.H[6 *
8602 cartesian_trajectory_planner_B.idx +
8603 cartesian_trajectory_planner_B.i_n];
8604 }
8605 }
8606 }
8607
8608 for (cartesian_trajectory_planner_B.b_i_o = 0;
8609 cartesian_trajectory_planner_B.b_i_o < 6;
8610 cartesian_trajectory_planner_B.b_i_o++) {
8611 for (cartesian_trajectory_planner_B.idx = 0;
8612 cartesian_trajectory_planner_B.idx < 6;
8613 cartesian_trajectory_planner_B.idx++) {
8614 cartesian_trajectory_planner_B.nx_k =
8615 cartesian_trajectory_planner_B.b_i_o + 6 *
8616 cartesian_trajectory_planner_B.idx;
8617 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_k]
8618 = 0.0;
8619 for (cartesian_trajectory_planner_B.i_n = 0;
8620 cartesian_trajectory_planner_B.i_n < 6;
8621 cartesian_trajectory_planner_B.i_n++) {
8622 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_k]
8623 += cartesian_trajectory_planner_B.H_m[6 *
8624 cartesian_trajectory_planner_B.i_n +
8625 cartesian_trajectory_planner_B.b_i_o] *
8626 cartesian_trajectory_planner_B.V[6 *
8627 cartesian_trajectory_planner_B.i_n +
8628 cartesian_trajectory_planner_B.idx];
8629 }
8630 }
8631 }
8632
8633 for (cartesian_trajectory_planner_B.b_i_o = 0;
8634 cartesian_trajectory_planner_B.b_i_o < 6;
8635 cartesian_trajectory_planner_B.b_i_o++) {
8636 for (cartesian_trajectory_planner_B.idx = 0;
8637 cartesian_trajectory_planner_B.idx < 6;
8638 cartesian_trajectory_planner_B.idx++) {
8639 cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.idx
8640 + 6 * cartesian_trajectory_planner_B.b_i_o] =
8641 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.idx]
8642 * cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i_o]
8643 / cartesian_trajectory_planner_B.beta;
8644 }
8645 }
8646
8647 for (cartesian_trajectory_planner_B.b_i_o = 0;
8648 cartesian_trajectory_planner_B.b_i_o < 36;
8649 cartesian_trajectory_planner_B.b_i_o++) {
8650 cartesian_trajectory_planner_B.s_e =
8651 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i_o]
8652 + cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.b_i_o];
8653 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i_o]
8654 = 1.4901161193847656E-8 * static_cast<real_T>
8655 (tmp_1[cartesian_trajectory_planner_B.b_i_o]) +
8656 cartesian_trajectory_planner_B.s_e;
8657 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i_o]
8658 = cartesian_trajectory_planner_B.s_e;
8659 }
8660
8661 if (!cartesian_tr_isPositiveDefinite
8662 (cartesian_trajectory_planner_B.H_m)) {
8663 *exitFlag = HessianNotPositiveSemidefinite;
8664 args = obj->ExtraArgs;
8665 for (cartesian_trajectory_planner_B.b_i_o = 0;
8666 cartesian_trajectory_planner_B.b_i_o < 36;
8667 cartesian_trajectory_planner_B.b_i_o++) {
8668 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
8669 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
8670 }
8671
8672 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8673 grad->size[0] = args->ErrTemp->size[0];
8674 cartes_emxEnsureCapacity_real_T(grad,
8675 cartesian_trajectory_planner_B.b_i_o);
8676 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
8677 for (cartesian_trajectory_planner_B.b_i_o = 0;
8678 cartesian_trajectory_planner_B.b_i_o <
8679 cartesian_trajectory_planner_B.i_n;
8680 cartesian_trajectory_planner_B.b_i_o++) {
8681 grad->data[cartesian_trajectory_planner_B.b_i_o] =
8682 args->ErrTemp->data[cartesian_trajectory_planner_B.b_i_o];
8683 }
8684
8685 for (cartesian_trajectory_planner_B.b_i_o = 0;
8686 cartesian_trajectory_planner_B.b_i_o < 6;
8687 cartesian_trajectory_planner_B.b_i_o++) {
8688 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8689 = 0.0;
8690 for (cartesian_trajectory_planner_B.idx = 0;
8691 cartesian_trajectory_planner_B.idx < 6;
8692 cartesian_trajectory_planner_B.idx++) {
8693 cartesian_trajectory_planner_B.A_d =
8694 cartesian_trajectory_planner_B.unusedU0[6 *
8695 cartesian_trajectory_planner_B.idx +
8696 cartesian_trajectory_planner_B.b_i_o] * grad->
8697 data[cartesian_trajectory_planner_B.idx] +
8698 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8699 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8700 = cartesian_trajectory_planner_B.A_d;
8701 }
8702 }
8703
8704 *err = cartesian_trajectory_pla_norm_a
8705 (cartesian_trajectory_planner_B.x);
8706 *iter = static_cast<real_T>
8707 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
8708 exitg2 = 1;
8709 } else {
8710 guard1 = true;
8711 }
8712 }
8713 }
8714
8715 if (guard1) {
8716 if (DampedBFGSwGradientProjectio_as(obj, xSol)) {
8717 for (cartesian_trajectory_planner_B.i_n = 0;
8718 cartesian_trajectory_planner_B.i_n < 6;
8719 cartesian_trajectory_planner_B.i_n++) {
8720 xSol[cartesian_trajectory_planner_B.i_n] =
8721 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_n];
8722 }
8723
8724 *exitFlag = SearchDirectionInvalid;
8725 args = obj->ExtraArgs;
8726 for (cartesian_trajectory_planner_B.b_i_o = 0;
8727 cartesian_trajectory_planner_B.b_i_o < 36;
8728 cartesian_trajectory_planner_B.b_i_o++) {
8729 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
8730 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
8731 }
8732
8733 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8734 grad->size[0] = args->ErrTemp->size[0];
8735 cartes_emxEnsureCapacity_real_T(grad,
8736 cartesian_trajectory_planner_B.b_i_o);
8737 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
8738 for (cartesian_trajectory_planner_B.b_i_o = 0;
8739 cartesian_trajectory_planner_B.b_i_o <
8740 cartesian_trajectory_planner_B.i_n;
8741 cartesian_trajectory_planner_B.b_i_o++) {
8742 grad->data[cartesian_trajectory_planner_B.b_i_o] = args->
8743 ErrTemp->data[cartesian_trajectory_planner_B.b_i_o];
8744 }
8745
8746 for (cartesian_trajectory_planner_B.b_i_o = 0;
8747 cartesian_trajectory_planner_B.b_i_o < 6;
8748 cartesian_trajectory_planner_B.b_i_o++) {
8749 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8750 = 0.0;
8751 for (cartesian_trajectory_planner_B.idx = 0;
8752 cartesian_trajectory_planner_B.idx < 6;
8753 cartesian_trajectory_planner_B.idx++) {
8754 cartesian_trajectory_planner_B.A_d =
8755 cartesian_trajectory_planner_B.unusedU0[6 *
8756 cartesian_trajectory_planner_B.idx +
8757 cartesian_trajectory_planner_B.b_i_o] * grad->
8758 data[cartesian_trajectory_planner_B.idx] +
8759 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8760 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8761 = cartesian_trajectory_planner_B.A_d;
8762 }
8763 }
8764
8765 *err = cartesian_trajectory_pla_norm_a
8766 (cartesian_trajectory_planner_B.x);
8767 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0)
8768 + 1.0;
8769 exitg2 = 1;
8770 } else {
8771 for (cartesian_trajectory_planner_B.i_n = 0;
8772 cartesian_trajectory_planner_B.i_n < 6;
8773 cartesian_trajectory_planner_B.i_n++) {
8774 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_n]
8775 = xSol[cartesian_trajectory_planner_B.i_n];
8776 }
8777
8778 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8779 grad->size[0] = alpha->size[0];
8780 cartes_emxEnsureCapacity_real_T(grad,
8781 cartesian_trajectory_planner_B.b_i_o);
8782 cartesian_trajectory_planner_B.i_n = alpha->size[0];
8783 for (cartesian_trajectory_planner_B.b_i_o = 0;
8784 cartesian_trajectory_planner_B.b_i_o <
8785 cartesian_trajectory_planner_B.i_n;
8786 cartesian_trajectory_planner_B.b_i_o++) {
8787 grad->data[cartesian_trajectory_planner_B.b_i_o] = alpha->
8788 data[cartesian_trajectory_planner_B.b_i_o];
8789 }
8790
8791 cartesian_trajectory_planner_B.cost =
8792 cartesian_trajectory_planner_B.costNew;
8793 cartesian_trajectory_planner_B.g_idx_0++;
8794 }
8795 }
8796 }
8797 }
8798 } else {
8799 *exitFlag = IterationLimitExceeded;
8800 args = obj->ExtraArgs;
8801 for (cartesian_trajectory_planner_B.b_i_o = 0;
8802 cartesian_trajectory_planner_B.b_i_o < 36;
8803 cartesian_trajectory_planner_B.b_i_o++) {
8804 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i_o]
8805 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i_o];
8806 }
8807
8808 cartesian_trajectory_planner_B.b_i_o = grad->size[0];
8809 grad->size[0] = args->ErrTemp->size[0];
8810 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i_o);
8811 cartesian_trajectory_planner_B.i_n = args->ErrTemp->size[0];
8812 for (cartesian_trajectory_planner_B.b_i_o = 0;
8813 cartesian_trajectory_planner_B.b_i_o <
8814 cartesian_trajectory_planner_B.i_n;
8815 cartesian_trajectory_planner_B.b_i_o++) {
8816 grad->data[cartesian_trajectory_planner_B.b_i_o] = args->ErrTemp->
8817 data[cartesian_trajectory_planner_B.b_i_o];
8818 }
8819
8820 for (cartesian_trajectory_planner_B.b_i_o = 0;
8821 cartesian_trajectory_planner_B.b_i_o < 6;
8822 cartesian_trajectory_planner_B.b_i_o++) {
8823 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o] =
8824 0.0;
8825 for (cartesian_trajectory_planner_B.idx = 0;
8826 cartesian_trajectory_planner_B.idx < 6;
8827 cartesian_trajectory_planner_B.idx++) {
8828 cartesian_trajectory_planner_B.A_d =
8829 cartesian_trajectory_planner_B.unusedU0[6 *
8830 cartesian_trajectory_planner_B.idx +
8831 cartesian_trajectory_planner_B.b_i_o] * grad->
8832 data[cartesian_trajectory_planner_B.idx] +
8833 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o];
8834 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i_o]
8835 = cartesian_trajectory_planner_B.A_d;
8836 }
8837 }
8838
8839 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
8840 *iter = obj->MaxNumIterationInternal;
8841 exitg2 = 1;
8842 }
8843 } while (exitg2 == 0);
8844
8845 cartesian_traje_emxFree_int32_T(&ii_3);
8846 cartesian_trajec_emxFree_real_T(&alpha_0);
8847 cartesian_trajec_emxFree_real_T(&A_3);
8848 cartesian_trajec_emxFree_real_T(&grad_1);
8849 cartesian_traje_emxFree_int32_T(&ii_2);
8850 cartesian_traje_emxFree_int32_T(&ii_1);
8851 cartesian_trajec_emxFree_real_T(&sNew);
8852 cartesian_trajec_emxFree_real_T(&grad_0);
8853 cartesian_trajec_emxFree_real_T(&tmp_0);
8854 cartesian_trajec_emxFree_real_T(&tmp);
8855 cartesian_trajec_emxFree_real_T(&sigma);
8856 cartesian_trajec_emxFree_real_T(&A_2);
8857 cartesian_tra_emxFree_boolean_T(&x);
8858 cartesian_trajec_emxFree_real_T(&y_0);
8859 cartesian_traje_emxFree_int32_T(&ii_0);
8860 cartesian_trajec_emxFree_real_T(&y);
8861 cartesian_traje_emxFree_int32_T(&ii);
8862 cartesian_trajec_emxFree_real_T(&a);
8863 cartesian_traje_emxFree_int32_T(&gb);
8864 cartesian_traje_emxFree_int32_T(&fb);
8865 cartesian_traje_emxFree_int32_T(&eb);
8866 cartesian_traje_emxFree_int32_T(&db);
8867 cartesian_traje_emxFree_int32_T(&cb);
8868 cartesian_trajec_emxFree_real_T(&L);
8869 cartesian_trajec_emxFree_real_T(&AIn);
8870 cartesian_trajec_emxFree_real_T(&alpha);
8871 cartesian_trajec_emxFree_real_T(&A);
8872 cartesian_tra_emxFree_boolean_T(&activeSet);
8873 cartesian_trajec_emxFree_real_T(&grad);
8874 cartesian_trajec_emxFree_real_T(&unusedU1);
8875}
8876
8877static void cartesian_trajectory_p_isfinite(const
8878 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b)
8879{
8880 emxArray_boolean_T_cartesian__T *tmp;
8881 int32_T loop_ub;
8882 int32_T i;
8883 i = b->size[0];
8884 b->size[0] = x->size[0];
8885 car_emxEnsureCapacity_boolean_T(b, i);
8886 loop_ub = x->size[0];
8887 for (i = 0; i < loop_ub; i++) {
8888 b->data[i] = rtIsInf(x->data[i]);
8889 }
8890
8891 cartesian_tra_emxInit_boolean_T(&tmp, 1);
8892 i = tmp->size[0];
8893 tmp->size[0] = x->size[0];
8894 car_emxEnsureCapacity_boolean_T(tmp, i);
8895 loop_ub = x->size[0];
8896 for (i = 0; i < loop_ub; i++) {
8897 tmp->data[i] = rtIsNaN(x->data[i]);
8898 }
8899
8900 i = b->size[0];
8901 car_emxEnsureCapacity_boolean_T(b, i);
8902 loop_ub = b->size[0];
8903 for (i = 0; i < loop_ub; i++) {
8904 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
8905 }
8906
8907 cartesian_tra_emxFree_boolean_T(&tmp);
8908}
8909
8910static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2])
8911{
8912 for (cartesian_trajectory_planner_B.b_j_m = 0;
8913 cartesian_trajectory_planner_B.b_j_m < 2;
8914 cartesian_trajectory_planner_B.b_j_m++) {
8915 cartesian_trajectory_planner_B.mti = mt[624] + 1U;
8916 if (cartesian_trajectory_planner_B.mti >= 625U) {
8917 for (cartesian_trajectory_planner_B.b_kk = 0;
8918 cartesian_trajectory_planner_B.b_kk < 227;
8919 cartesian_trajectory_planner_B.b_kk++) {
8920 cartesian_trajectory_planner_B.y_in =
8921 (mt[cartesian_trajectory_planner_B.b_kk + 1] & 2147483647U) |
8922 (mt[cartesian_trajectory_planner_B.b_kk] & 2147483648U);
8923 if ((cartesian_trajectory_planner_B.y_in & 1U) == 0U) {
8924 cartesian_trajectory_planner_B.y_in >>= 1U;
8925 } else {
8926 cartesian_trajectory_planner_B.y_in =
8927 cartesian_trajectory_planner_B.y_in >> 1U ^ 2567483615U;
8928 }
8929
8930 mt[cartesian_trajectory_planner_B.b_kk] =
8931 mt[cartesian_trajectory_planner_B.b_kk + 397] ^
8932 cartesian_trajectory_planner_B.y_in;
8933 }
8934
8935 for (cartesian_trajectory_planner_B.b_kk = 0;
8936 cartesian_trajectory_planner_B.b_kk < 396;
8937 cartesian_trajectory_planner_B.b_kk++) {
8938 cartesian_trajectory_planner_B.y_in =
8939 (mt[cartesian_trajectory_planner_B.b_kk + 227] & 2147483648U) |
8940 (mt[cartesian_trajectory_planner_B.b_kk + 228] & 2147483647U);
8941 if ((cartesian_trajectory_planner_B.y_in & 1U) == 0U) {
8942 cartesian_trajectory_planner_B.y_in >>= 1U;
8943 } else {
8944 cartesian_trajectory_planner_B.y_in =
8945 cartesian_trajectory_planner_B.y_in >> 1U ^ 2567483615U;
8946 }
8947
8948 mt[cartesian_trajectory_planner_B.b_kk + 227] =
8949 mt[cartesian_trajectory_planner_B.b_kk] ^
8950 cartesian_trajectory_planner_B.y_in;
8951 }
8952
8953 cartesian_trajectory_planner_B.y_in = (mt[623] & 2147483648U) | (mt[0] &
8954 2147483647U);
8955 if ((cartesian_trajectory_planner_B.y_in & 1U) == 0U) {
8956 cartesian_trajectory_planner_B.y_in >>= 1U;
8957 } else {
8958 cartesian_trajectory_planner_B.y_in =
8959 cartesian_trajectory_planner_B.y_in >> 1U ^ 2567483615U;
8960 }
8961
8962 mt[623] = mt[396] ^ cartesian_trajectory_planner_B.y_in;
8963 cartesian_trajectory_planner_B.mti = 1U;
8964 }
8965
8966 cartesian_trajectory_planner_B.y_in = mt[static_cast<int32_T>
8967 (cartesian_trajectory_planner_B.mti) - 1];
8968 mt[624] = cartesian_trajectory_planner_B.mti;
8969 cartesian_trajectory_planner_B.y_in ^= cartesian_trajectory_planner_B.y_in >>
8970 11U;
8971 cartesian_trajectory_planner_B.y_in ^= cartesian_trajectory_planner_B.y_in <<
8972 7U & 2636928640U;
8973 cartesian_trajectory_planner_B.y_in ^= cartesian_trajectory_planner_B.y_in <<
8974 15U & 4022730752U;
8975 u[cartesian_trajectory_planner_B.b_j_m] =
8976 cartesian_trajectory_planner_B.y_in >> 18U ^
8977 cartesian_trajectory_planner_B.y_in;
8978 }
8979}
8980
8981static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625])
8982{
8983 boolean_T isvalid;
8984 boolean_T exitg1;
8985 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
8986 isvalid = true;
8987 } else {
8988 isvalid = false;
8989 }
8990
8991 if (isvalid) {
8992 isvalid = false;
8993 cartesian_trajectory_planner_B.k_a = 0;
8994 exitg1 = false;
8995 while ((!exitg1) && (cartesian_trajectory_planner_B.k_a + 1 < 625)) {
8996 if (mt[cartesian_trajectory_planner_B.k_a] == 0U) {
8997 cartesian_trajectory_planner_B.k_a++;
8998 } else {
8999 isvalid = true;
9000 exitg1 = true;
9001 }
9002 }
9003 }
9004
9005 return isvalid;
9006}
9007
9008static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625])
9009{
9010 real_T r;
9011 int32_T exitg1;
9012
9013 // ========================= COPYRIGHT NOTICE ============================
9014 // This is a uniform (0,1) pseudorandom number generator based on:
9015 //
9016 // A C-program for MT19937, with initialization improved 2002/1/26.
9017 // Coded by Takuji Nishimura and Makoto Matsumoto.
9018 //
9019 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9020 // All rights reserved.
9021 //
9022 // Redistribution and use in source and binary forms, with or without
9023 // modification, are permitted provided that the following conditions
9024 // are met:
9025 //
9026 // 1. Redistributions of source code must retain the above copyright
9027 // notice, this list of conditions and the following disclaimer.
9028 //
9029 // 2. Redistributions in binary form must reproduce the above copyright
9030 // notice, this list of conditions and the following disclaimer
9031 // in the documentation and/or other materials provided with the
9032 // distribution.
9033 //
9034 // 3. The names of its contributors may not be used to endorse or
9035 // promote products derived from this software without specific
9036 // prior written permission.
9037 //
9038 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9039 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9040 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9041 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9042 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9043 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9044 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9045 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9046 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9047 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9048 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9049 //
9050 // ============================= END =================================
9051 do {
9052 exitg1 = 0;
9053 cartesi_genrand_uint32_vector_a(mt, cartesian_trajectory_planner_B.b_u_b);
9054 r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u_b[0] >> 5U) *
9055 6.7108864E+7 + static_cast<real_T>
9056 (cartesian_trajectory_planner_B.b_u_b[1] >> 6U)) *
9057 1.1102230246251565E-16;
9058 if (r == 0.0) {
9059 if (!cartesian_trajec_is_valid_state(mt)) {
9060 cartesian_trajectory_planner_B.r_a = 5489U;
9061 mt[0] = 5489U;
9062 for (cartesian_trajectory_planner_B.b_mti_m = 0;
9063 cartesian_trajectory_planner_B.b_mti_m < 623;
9064 cartesian_trajectory_planner_B.b_mti_m++) {
9065 cartesian_trajectory_planner_B.r_a =
9066 ((cartesian_trajectory_planner_B.r_a >> 30U ^
9067 cartesian_trajectory_planner_B.r_a) * 1812433253U +
9068 cartesian_trajectory_planner_B.b_mti_m) + 1U;
9069 mt[cartesian_trajectory_planner_B.b_mti_m + 1] =
9070 cartesian_trajectory_planner_B.r_a;
9071 }
9072
9073 mt[624] = 624U;
9074 }
9075 } else {
9076 exitg1 = 1;
9077 }
9078 } while (exitg1 == 0);
9079
9080 return r;
9081}
9082
9083static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625])
9084{
9085 real_T r;
9086 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
9087 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
9088 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
9089 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
9090 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
9091 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
9092 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
9093 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
9094 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
9095 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
9096 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
9097 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
9098 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
9099 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
9100 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
9101 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
9102 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
9103 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
9104 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
9105 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
9106 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
9107 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
9108 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
9109 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
9110 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
9111 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
9112 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
9113 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
9114 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
9115 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
9116 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
9117 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
9118 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
9119 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
9120 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
9121 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
9122 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
9123 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
9124 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
9125 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
9126 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
9127 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
9128 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
9129 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
9130 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
9131 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
9132 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
9133 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
9134 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
9135 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
9136 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
9137 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
9138 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
9139 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
9140 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
9141 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
9142 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
9143 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
9144 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
9145 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
9146 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
9147 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
9148 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
9149 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
9150 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
9151 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
9152 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
9153 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
9154 0.000477467764609386 };
9155
9156 const real_T *fitab;
9157 int32_T exitg1;
9158 cartesian_trajectory_planner_B.xi[0] = 0.0;
9159 cartesian_trajectory_planner_B.xi[1] = 0.215241895984875;
9160 cartesian_trajectory_planner_B.xi[2] = 0.286174591792068;
9161 cartesian_trajectory_planner_B.xi[3] = 0.335737519214422;
9162 cartesian_trajectory_planner_B.xi[4] = 0.375121332878378;
9163 cartesian_trajectory_planner_B.xi[5] = 0.408389134611989;
9164 cartesian_trajectory_planner_B.xi[6] = 0.43751840220787;
9165 cartesian_trajectory_planner_B.xi[7] = 0.46363433679088;
9166 cartesian_trajectory_planner_B.xi[8] = 0.487443966139235;
9167 cartesian_trajectory_planner_B.xi[9] = 0.50942332960209;
9168 cartesian_trajectory_planner_B.xi[10] = 0.529909720661557;
9169 cartesian_trajectory_planner_B.xi[11] = 0.549151702327164;
9170 cartesian_trajectory_planner_B.xi[12] = 0.567338257053817;
9171 cartesian_trajectory_planner_B.xi[13] = 0.584616766106378;
9172 cartesian_trajectory_planner_B.xi[14] = 0.601104617755991;
9173 cartesian_trajectory_planner_B.xi[15] = 0.61689699000775;
9174 cartesian_trajectory_planner_B.xi[16] = 0.63207223638606;
9175 cartesian_trajectory_planner_B.xi[17] = 0.646695714894993;
9176 cartesian_trajectory_planner_B.xi[18] = 0.660822574244419;
9177 cartesian_trajectory_planner_B.xi[19] = 0.674499822837293;
9178 cartesian_trajectory_planner_B.xi[20] = 0.687767892795788;
9179 cartesian_trajectory_planner_B.xi[21] = 0.700661841106814;
9180 cartesian_trajectory_planner_B.xi[22] = 0.713212285190975;
9181 cartesian_trajectory_planner_B.xi[23] = 0.725446140909999;
9182 cartesian_trajectory_planner_B.xi[24] = 0.737387211434295;
9183 cartesian_trajectory_planner_B.xi[25] = 0.749056662017815;
9184 cartesian_trajectory_planner_B.xi[26] = 0.760473406430107;
9185 cartesian_trajectory_planner_B.xi[27] = 0.771654424224568;
9186 cartesian_trajectory_planner_B.xi[28] = 0.782615023307232;
9187 cartesian_trajectory_planner_B.xi[29] = 0.793369058840623;
9188 cartesian_trajectory_planner_B.xi[30] = 0.80392911698997;
9189 cartesian_trajectory_planner_B.xi[31] = 0.814306670135215;
9190 cartesian_trajectory_planner_B.xi[32] = 0.824512208752291;
9191 cartesian_trajectory_planner_B.xi[33] = 0.834555354086381;
9192 cartesian_trajectory_planner_B.xi[34] = 0.844444954909153;
9193 cartesian_trajectory_planner_B.xi[35] = 0.854189171008163;
9194 cartesian_trajectory_planner_B.xi[36] = 0.863795545553308;
9195 cartesian_trajectory_planner_B.xi[37] = 0.87327106808886;
9196 cartesian_trajectory_planner_B.xi[38] = 0.882622229585165;
9197 cartesian_trajectory_planner_B.xi[39] = 0.891855070732941;
9198 cartesian_trajectory_planner_B.xi[40] = 0.900975224461221;
9199 cartesian_trajectory_planner_B.xi[41] = 0.909987953496718;
9200 cartesian_trajectory_planner_B.xi[42] = 0.91889818364959;
9201 cartesian_trajectory_planner_B.xi[43] = 0.927710533401999;
9202 cartesian_trajectory_planner_B.xi[44] = 0.936429340286575;
9203 cartesian_trajectory_planner_B.xi[45] = 0.945058684468165;
9204 cartesian_trajectory_planner_B.xi[46] = 0.953602409881086;
9205 cartesian_trajectory_planner_B.xi[47] = 0.96206414322304;
9206 cartesian_trajectory_planner_B.xi[48] = 0.970447311064224;
9207 cartesian_trajectory_planner_B.xi[49] = 0.978755155294224;
9208 cartesian_trajectory_planner_B.xi[50] = 0.986990747099062;
9209 cartesian_trajectory_planner_B.xi[51] = 0.99515699963509;
9210 cartesian_trajectory_planner_B.xi[52] = 1.00325667954467;
9211 cartesian_trajectory_planner_B.xi[53] = 1.01129241744;
9212 cartesian_trajectory_planner_B.xi[54] = 1.01926671746548;
9213 cartesian_trajectory_planner_B.xi[55] = 1.02718196603564;
9214 cartesian_trajectory_planner_B.xi[56] = 1.03504043983344;
9215 cartesian_trajectory_planner_B.xi[57] = 1.04284431314415;
9216 cartesian_trajectory_planner_B.xi[58] = 1.05059566459093;
9217 cartesian_trajectory_planner_B.xi[59] = 1.05829648333067;
9218 cartesian_trajectory_planner_B.xi[60] = 1.06594867476212;
9219 cartesian_trajectory_planner_B.xi[61] = 1.07355406579244;
9220 cartesian_trajectory_planner_B.xi[62] = 1.0811144097034;
9221 cartesian_trajectory_planner_B.xi[63] = 1.08863139065398;
9222 cartesian_trajectory_planner_B.xi[64] = 1.09610662785202;
9223 cartesian_trajectory_planner_B.xi[65] = 1.10354167942464;
9224 cartesian_trajectory_planner_B.xi[66] = 1.11093804601357;
9225 cartesian_trajectory_planner_B.xi[67] = 1.11829717411934;
9226 cartesian_trajectory_planner_B.xi[68] = 1.12562045921553;
9227 cartesian_trajectory_planner_B.xi[69] = 1.13290924865253;
9228 cartesian_trajectory_planner_B.xi[70] = 1.14016484436815;
9229 cartesian_trajectory_planner_B.xi[71] = 1.14738850542085;
9230 cartesian_trajectory_planner_B.xi[72] = 1.15458145035993;
9231 cartesian_trajectory_planner_B.xi[73] = 1.16174485944561;
9232 cartesian_trajectory_planner_B.xi[74] = 1.16887987673083;
9233 cartesian_trajectory_planner_B.xi[75] = 1.17598761201545;
9234 cartesian_trajectory_planner_B.xi[76] = 1.18306914268269;
9235 cartesian_trajectory_planner_B.xi[77] = 1.19012551542669;
9236 cartesian_trajectory_planner_B.xi[78] = 1.19715774787944;
9237 cartesian_trajectory_planner_B.xi[79] = 1.20416683014438;
9238 cartesian_trajectory_planner_B.xi[80] = 1.2111537262437;
9239 cartesian_trajectory_planner_B.xi[81] = 1.21811937548548;
9240 cartesian_trajectory_planner_B.xi[82] = 1.22506469375653;
9241 cartesian_trajectory_planner_B.xi[83] = 1.23199057474614;
9242 cartesian_trajectory_planner_B.xi[84] = 1.23889789110569;
9243 cartesian_trajectory_planner_B.xi[85] = 1.24578749554863;
9244 cartesian_trajectory_planner_B.xi[86] = 1.2526602218949;
9245 cartesian_trajectory_planner_B.xi[87] = 1.25951688606371;
9246 cartesian_trajectory_planner_B.xi[88] = 1.26635828701823;
9247 cartesian_trajectory_planner_B.xi[89] = 1.27318520766536;
9248 cartesian_trajectory_planner_B.xi[90] = 1.27999841571382;
9249 cartesian_trajectory_planner_B.xi[91] = 1.28679866449324;
9250 cartesian_trajectory_planner_B.xi[92] = 1.29358669373695;
9251 cartesian_trajectory_planner_B.xi[93] = 1.30036323033084;
9252 cartesian_trajectory_planner_B.xi[94] = 1.30712898903073;
9253 cartesian_trajectory_planner_B.xi[95] = 1.31388467315022;
9254 cartesian_trajectory_planner_B.xi[96] = 1.32063097522106;
9255 cartesian_trajectory_planner_B.xi[97] = 1.32736857762793;
9256 cartesian_trajectory_planner_B.xi[98] = 1.33409815321936;
9257 cartesian_trajectory_planner_B.xi[99] = 1.3408203658964;
9258 cartesian_trajectory_planner_B.xi[100] = 1.34753587118059;
9259 cartesian_trajectory_planner_B.xi[101] = 1.35424531676263;
9260 cartesian_trajectory_planner_B.xi[102] = 1.36094934303328;
9261 cartesian_trajectory_planner_B.xi[103] = 1.36764858359748;
9262 cartesian_trajectory_planner_B.xi[104] = 1.37434366577317;
9263 cartesian_trajectory_planner_B.xi[105] = 1.38103521107586;
9264 cartesian_trajectory_planner_B.xi[106] = 1.38772383568998;
9265 cartesian_trajectory_planner_B.xi[107] = 1.39441015092814;
9266 cartesian_trajectory_planner_B.xi[108] = 1.40109476367925;
9267 cartesian_trajectory_planner_B.xi[109] = 1.4077782768464;
9268 cartesian_trajectory_planner_B.xi[110] = 1.41446128977547;
9269 cartesian_trajectory_planner_B.xi[111] = 1.42114439867531;
9270 cartesian_trajectory_planner_B.xi[112] = 1.42782819703026;
9271 cartesian_trajectory_planner_B.xi[113] = 1.43451327600589;
9272 cartesian_trajectory_planner_B.xi[114] = 1.44120022484872;
9273 cartesian_trajectory_planner_B.xi[115] = 1.44788963128058;
9274 cartesian_trajectory_planner_B.xi[116] = 1.45458208188841;
9275 cartesian_trajectory_planner_B.xi[117] = 1.46127816251028;
9276 cartesian_trajectory_planner_B.xi[118] = 1.46797845861808;
9277 cartesian_trajectory_planner_B.xi[119] = 1.47468355569786;
9278 cartesian_trajectory_planner_B.xi[120] = 1.48139403962819;
9279 cartesian_trajectory_planner_B.xi[121] = 1.48811049705745;
9280 cartesian_trajectory_planner_B.xi[122] = 1.49483351578049;
9281 cartesian_trajectory_planner_B.xi[123] = 1.50156368511546;
9282 cartesian_trajectory_planner_B.xi[124] = 1.50830159628131;
9283 cartesian_trajectory_planner_B.xi[125] = 1.51504784277671;
9284 cartesian_trajectory_planner_B.xi[126] = 1.521803020761;
9285 cartesian_trajectory_planner_B.xi[127] = 1.52856772943771;
9286 cartesian_trajectory_planner_B.xi[128] = 1.53534257144151;
9287 cartesian_trajectory_planner_B.xi[129] = 1.542128153229;
9288 cartesian_trajectory_planner_B.xi[130] = 1.54892508547417;
9289 cartesian_trajectory_planner_B.xi[131] = 1.55573398346918;
9290 cartesian_trajectory_planner_B.xi[132] = 1.56255546753104;
9291 cartesian_trajectory_planner_B.xi[133] = 1.56939016341512;
9292 cartesian_trajectory_planner_B.xi[134] = 1.57623870273591;
9293 cartesian_trajectory_planner_B.xi[135] = 1.58310172339603;
9294 cartesian_trajectory_planner_B.xi[136] = 1.58997987002419;
9295 cartesian_trajectory_planner_B.xi[137] = 1.59687379442279;
9296 cartesian_trajectory_planner_B.xi[138] = 1.60378415602609;
9297 cartesian_trajectory_planner_B.xi[139] = 1.61071162236983;
9298 cartesian_trajectory_planner_B.xi[140] = 1.61765686957301;
9299 cartesian_trajectory_planner_B.xi[141] = 1.62462058283303;
9300 cartesian_trajectory_planner_B.xi[142] = 1.63160345693487;
9301 cartesian_trajectory_planner_B.xi[143] = 1.63860619677555;
9302 cartesian_trajectory_planner_B.xi[144] = 1.64562951790478;
9303 cartesian_trajectory_planner_B.xi[145] = 1.65267414708306;
9304 cartesian_trajectory_planner_B.xi[146] = 1.65974082285818;
9305 cartesian_trajectory_planner_B.xi[147] = 1.66683029616166;
9306 cartesian_trajectory_planner_B.xi[148] = 1.67394333092612;
9307 cartesian_trajectory_planner_B.xi[149] = 1.68108070472517;
9308 cartesian_trajectory_planner_B.xi[150] = 1.68824320943719;
9309 cartesian_trajectory_planner_B.xi[151] = 1.69543165193456;
9310 cartesian_trajectory_planner_B.xi[152] = 1.70264685479992;
9311 cartesian_trajectory_planner_B.xi[153] = 1.7098896570713;
9312 cartesian_trajectory_planner_B.xi[154] = 1.71716091501782;
9313 cartesian_trajectory_planner_B.xi[155] = 1.72446150294804;
9314 cartesian_trajectory_planner_B.xi[156] = 1.73179231405296;
9315 cartesian_trajectory_planner_B.xi[157] = 1.73915426128591;
9316 cartesian_trajectory_planner_B.xi[158] = 1.74654827828172;
9317 cartesian_trajectory_planner_B.xi[159] = 1.75397532031767;
9318 cartesian_trajectory_planner_B.xi[160] = 1.76143636531891;
9319 cartesian_trajectory_planner_B.xi[161] = 1.76893241491127;
9320 cartesian_trajectory_planner_B.xi[162] = 1.77646449552452;
9321 cartesian_trajectory_planner_B.xi[163] = 1.78403365954944;
9322 cartesian_trajectory_planner_B.xi[164] = 1.79164098655216;
9323 cartesian_trajectory_planner_B.xi[165] = 1.79928758454972;
9324 cartesian_trajectory_planner_B.xi[166] = 1.80697459135082;
9325 cartesian_trajectory_planner_B.xi[167] = 1.81470317596628;
9326 cartesian_trajectory_planner_B.xi[168] = 1.82247454009388;
9327 cartesian_trajectory_planner_B.xi[169] = 1.83028991968276;
9328 cartesian_trajectory_planner_B.xi[170] = 1.83815058658281;
9329 cartesian_trajectory_planner_B.xi[171] = 1.84605785028518;
9330 cartesian_trajectory_planner_B.xi[172] = 1.8540130597602;
9331 cartesian_trajectory_planner_B.xi[173] = 1.86201760539967;
9332 cartesian_trajectory_planner_B.xi[174] = 1.87007292107127;
9333 cartesian_trajectory_planner_B.xi[175] = 1.878180486293;
9334 cartesian_trajectory_planner_B.xi[176] = 1.88634182853678;
9335 cartesian_trajectory_planner_B.xi[177] = 1.8945585256707;
9336 cartesian_trajectory_planner_B.xi[178] = 1.90283220855043;
9337 cartesian_trajectory_planner_B.xi[179] = 1.91116456377125;
9338 cartesian_trajectory_planner_B.xi[180] = 1.91955733659319;
9339 cartesian_trajectory_planner_B.xi[181] = 1.92801233405266;
9340 cartesian_trajectory_planner_B.xi[182] = 1.93653142827569;
9341 cartesian_trajectory_planner_B.xi[183] = 1.94511656000868;
9342 cartesian_trajectory_planner_B.xi[184] = 1.95376974238465;
9343 cartesian_trajectory_planner_B.xi[185] = 1.96249306494436;
9344 cartesian_trajectory_planner_B.xi[186] = 1.97128869793366;
9345 cartesian_trajectory_planner_B.xi[187] = 1.98015889690048;
9346 cartesian_trajectory_planner_B.xi[188] = 1.98910600761744;
9347 cartesian_trajectory_planner_B.xi[189] = 1.99813247135842;
9348 cartesian_trajectory_planner_B.xi[190] = 2.00724083056053;
9349 cartesian_trajectory_planner_B.xi[191] = 2.0164337349062;
9350 cartesian_trajectory_planner_B.xi[192] = 2.02571394786385;
9351 cartesian_trajectory_planner_B.xi[193] = 2.03508435372962;
9352 cartesian_trajectory_planner_B.xi[194] = 2.04454796521753;
9353 cartesian_trajectory_planner_B.xi[195] = 2.05410793165065;
9354 cartesian_trajectory_planner_B.xi[196] = 2.06376754781173;
9355 cartesian_trajectory_planner_B.xi[197] = 2.07353026351874;
9356 cartesian_trajectory_planner_B.xi[198] = 2.0833996939983;
9357 cartesian_trajectory_planner_B.xi[199] = 2.09337963113879;
9358 cartesian_trajectory_planner_B.xi[200] = 2.10347405571488;
9359 cartesian_trajectory_planner_B.xi[201] = 2.11368715068665;
9360 cartesian_trajectory_planner_B.xi[202] = 2.12402331568952;
9361 cartesian_trajectory_planner_B.xi[203] = 2.13448718284602;
9362 cartesian_trajectory_planner_B.xi[204] = 2.14508363404789;
9363 cartesian_trajectory_planner_B.xi[205] = 2.15581781987674;
9364 cartesian_trajectory_planner_B.xi[206] = 2.16669518035431;
9365 cartesian_trajectory_planner_B.xi[207] = 2.17772146774029;
9366 cartesian_trajectory_planner_B.xi[208] = 2.18890277162636;
9367 cartesian_trajectory_planner_B.xi[209] = 2.20024554661128;
9368 cartesian_trajectory_planner_B.xi[210] = 2.21175664288416;
9369 cartesian_trajectory_planner_B.xi[211] = 2.22344334009251;
9370 cartesian_trajectory_planner_B.xi[212] = 2.23531338492992;
9371 cartesian_trajectory_planner_B.xi[213] = 2.24737503294739;
9372 cartesian_trajectory_planner_B.xi[214] = 2.25963709517379;
9373 cartesian_trajectory_planner_B.xi[215] = 2.27210899022838;
9374 cartesian_trajectory_planner_B.xi[216] = 2.28480080272449;
9375 cartesian_trajectory_planner_B.xi[217] = 2.29772334890286;
9376 cartesian_trajectory_planner_B.xi[218] = 2.31088825060137;
9377 cartesian_trajectory_planner_B.xi[219] = 2.32430801887113;
9378 cartesian_trajectory_planner_B.xi[220] = 2.33799614879653;
9379 cartesian_trajectory_planner_B.xi[221] = 2.35196722737914;
9380 cartesian_trajectory_planner_B.xi[222] = 2.36623705671729;
9381 cartesian_trajectory_planner_B.xi[223] = 2.38082279517208;
9382 cartesian_trajectory_planner_B.xi[224] = 2.39574311978193;
9383 cartesian_trajectory_planner_B.xi[225] = 2.41101841390112;
9384 cartesian_trajectory_planner_B.xi[226] = 2.42667098493715;
9385 cartesian_trajectory_planner_B.xi[227] = 2.44272531820036;
9386 cartesian_trajectory_planner_B.xi[228] = 2.4592083743347;
9387 cartesian_trajectory_planner_B.xi[229] = 2.47614993967052;
9388 cartesian_trajectory_planner_B.xi[230] = 2.49358304127105;
9389 cartesian_trajectory_planner_B.xi[231] = 2.51154444162669;
9390 cartesian_trajectory_planner_B.xi[232] = 2.53007523215985;
9391 cartesian_trajectory_planner_B.xi[233] = 2.54922155032478;
9392 cartesian_trajectory_planner_B.xi[234] = 2.56903545268184;
9393 cartesian_trajectory_planner_B.xi[235] = 2.58957598670829;
9394 cartesian_trajectory_planner_B.xi[236] = 2.61091051848882;
9395 cartesian_trajectory_planner_B.xi[237] = 2.63311639363158;
9396 cartesian_trajectory_planner_B.xi[238] = 2.65628303757674;
9397 cartesian_trajectory_planner_B.xi[239] = 2.68051464328574;
9398 cartesian_trajectory_planner_B.xi[240] = 2.70593365612306;
9399 cartesian_trajectory_planner_B.xi[241] = 2.73268535904401;
9400 cartesian_trajectory_planner_B.xi[242] = 2.76094400527999;
9401 cartesian_trajectory_planner_B.xi[243] = 2.79092117400193;
9402 cartesian_trajectory_planner_B.xi[244] = 2.82287739682644;
9403 cartesian_trajectory_planner_B.xi[245] = 2.85713873087322;
9404 cartesian_trajectory_planner_B.xi[246] = 2.89412105361341;
9405 cartesian_trajectory_planner_B.xi[247] = 2.93436686720889;
9406 cartesian_trajectory_planner_B.xi[248] = 2.97860327988184;
9407 cartesian_trajectory_planner_B.xi[249] = 3.02783779176959;
9408 cartesian_trajectory_planner_B.xi[250] = 3.08352613200214;
9409 cartesian_trajectory_planner_B.xi[251] = 3.147889289518;
9410 cartesian_trajectory_planner_B.xi[252] = 3.2245750520478;
9411 cartesian_trajectory_planner_B.xi[253] = 3.32024473383983;
9412 cartesian_trajectory_planner_B.xi[254] = 3.44927829856143;
9413 cartesian_trajectory_planner_B.xi[255] = 3.65415288536101;
9414 cartesian_trajectory_planner_B.xi[256] = 3.91075795952492;
9415 fitab = &tmp[0];
9416 do {
9417 exitg1 = 0;
9418 cartesi_genrand_uint32_vector_a(state, cartesian_trajectory_planner_B.u32);
9419 cartesian_trajectory_planner_B.i_f = static_cast<int32_T>
9420 ((cartesian_trajectory_planner_B.u32[1] >> 24U) + 1U);
9421 r = ((static_cast<real_T>(cartesian_trajectory_planner_B.u32[0] >> 3U) *
9422 1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>
9423 (cartesian_trajectory_planner_B.u32[1]) & 16777215)) *
9424 2.2204460492503131E-16 - 1.0) *
9425 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_f];
9426 if (fabs(r) <=
9427 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_f - 1])
9428 {
9429 exitg1 = 1;
9430 } else if (cartesian_trajectory_planner_B.i_f < 256) {
9431 cartesian_trajectory_planner_B.x_o = cartesian_trajectory_genrandu_a(state);
9432 if ((fitab[cartesian_trajectory_planner_B.i_f - 1] -
9433 fitab[cartesian_trajectory_planner_B.i_f]) *
9434 cartesian_trajectory_planner_B.x_o +
9435 fitab[cartesian_trajectory_planner_B.i_f] < exp(-0.5 * r * r)) {
9436 exitg1 = 1;
9437 }
9438 } else {
9439 do {
9440 cartesian_trajectory_planner_B.x_o = cartesian_trajectory_genrandu_a
9441 (state);
9442 cartesian_trajectory_planner_B.x_o = log
9443 (cartesian_trajectory_planner_B.x_o) * 0.273661237329758;
9444 cartesian_trajectory_planner_B.d_u = cartesian_trajectory_genrandu_a
9445 (state);
9446 } while (!(-2.0 * log(cartesian_trajectory_planner_B.d_u) >
9447 cartesian_trajectory_planner_B.x_o *
9448 cartesian_trajectory_planner_B.x_o));
9449
9450 if (r < 0.0) {
9451 r = cartesian_trajectory_planner_B.x_o - 3.65415288536101;
9452 } else {
9453 r = 3.65415288536101 - cartesian_trajectory_planner_B.x_o;
9454 }
9455
9456 exitg1 = 1;
9457 }
9458 } while (exitg1 == 0);
9459
9460 return r;
9461}
9462
9463static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
9464 emxArray_real_T_cartesian_tra_T *r)
9465{
9466 cartesian_trajectory_planner_B.b_k_o4 = r->size[0] * r->size[1];
9467 r->size[0] = static_cast<int32_T>(varargin_1[0]);
9468 r->size[1] = 1;
9469 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_o4);
9470 cartesian_trajectory_planner_B.d_j = r->size[0] - 1;
9471 for (cartesian_trajectory_planner_B.b_k_o4 = 0;
9472 cartesian_trajectory_planner_B.b_k_o4 <=
9473 cartesian_trajectory_planner_B.d_j; cartesian_trajectory_planner_B.b_k_o4
9474 ++) {
9475 r->data[cartesian_trajectory_planner_B.b_k_o4] =
9476 cartesia_eml_rand_mt19937ar_ast(cartesian_trajectory_planner_DW.state_m);
9477 }
9478}
9479
9480static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
9481 b_state[625], real_T *r)
9482{
9483 int32_T exitg1;
9484 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
9485
9486 // ========================= COPYRIGHT NOTICE ============================
9487 // This is a uniform (0,1) pseudorandom number generator based on:
9488 //
9489 // A C-program for MT19937, with initialization improved 2002/1/26.
9490 // Coded by Takuji Nishimura and Makoto Matsumoto.
9491 //
9492 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9493 // All rights reserved.
9494 //
9495 // Redistribution and use in source and binary forms, with or without
9496 // modification, are permitted provided that the following conditions
9497 // are met:
9498 //
9499 // 1. Redistributions of source code must retain the above copyright
9500 // notice, this list of conditions and the following disclaimer.
9501 //
9502 // 2. Redistributions in binary form must reproduce the above copyright
9503 // notice, this list of conditions and the following disclaimer
9504 // in the documentation and/or other materials provided with the
9505 // distribution.
9506 //
9507 // 3. The names of its contributors may not be used to endorse or
9508 // promote products derived from this software without specific
9509 // prior written permission.
9510 //
9511 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9512 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9513 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9514 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9515 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9516 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9517 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9518 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9519 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9520 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9521 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9522 //
9523 // ============================= END =================================
9524 do {
9525 exitg1 = 0;
9526 cartesi_genrand_uint32_vector_a(b_state, cartesian_trajectory_planner_B.b_u);
9527 *r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u[0] >> 5U) *
9528 6.7108864E+7 + static_cast<real_T>(cartesian_trajectory_planner_B.b_u
9529 [1] >> 6U)) * 1.1102230246251565E-16;
9530 if (*r == 0.0) {
9531 if (!cartesian_trajec_is_valid_state(b_state)) {
9532 cartesian_trajectory_planner_B.r = 5489U;
9533 b_state[0] = 5489U;
9534 for (cartesian_trajectory_planner_B.b_mti = 0;
9535 cartesian_trajectory_planner_B.b_mti < 623;
9536 cartesian_trajectory_planner_B.b_mti++) {
9537 cartesian_trajectory_planner_B.r = ((cartesian_trajectory_planner_B.r >>
9538 30U ^ cartesian_trajectory_planner_B.r) * 1812433253U +
9539 cartesian_trajectory_planner_B.b_mti) + 1U;
9540 b_state[cartesian_trajectory_planner_B.b_mti + 1] =
9541 cartesian_trajectory_planner_B.r;
9542 }
9543
9544 b_state[624] = 624U;
9545 }
9546 } else {
9547 exitg1 = 1;
9548 }
9549 } while (exitg1 == 0);
9550}
9551
9552static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
9553 emxArray_real_T_cartesian_tra_T *r)
9554{
9555 cartesian_trajectory_planner_B.b_k_o = r->size[0];
9556 cartesian_trajectory_planner_B.d_f = static_cast<int32_T>(varargin_1);
9557 r->size[0] = cartesian_trajectory_planner_B.d_f;
9558 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_o);
9559 cartesian_trajectory_planner_B.d_f--;
9560 for (cartesian_trajectory_planner_B.b_k_o = 0;
9561 cartesian_trajectory_planner_B.b_k_o <=
9562 cartesian_trajectory_planner_B.d_f; cartesian_trajectory_planner_B.b_k_o
9563 ++) {
9564 memcpy(&cartesian_trajectory_planner_B.uv1[0],
9565 &cartesian_trajectory_planner_DW.state_m[0], 625U * sizeof(uint32_T));
9566 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv1,
9567 cartesian_trajectory_planner_DW.state_m, &r->
9568 data[cartesian_trajectory_planner_B.b_k_o]);
9569 }
9570}
9571
9572static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
9573 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
9574 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
9575 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
9576 solutionInfo_Status_size[2])
9577{
9578 emxArray_real_T_cartesian_tra_T *newseed;
9579 f_robotics_manip_internal_IKE_T *args;
9580 x_robotics_manip_internal_Rig_T *obj_0;
9581 emxArray_real_T_cartesian_tra_T *qi;
9582 c_rigidBodyJoint_cartesian_as_T *obj_1;
9583 emxArray_real_T_cartesian_tra_T *ub;
9584 emxArray_real_T_cartesian_tra_T *lb;
9585 emxArray_real_T_cartesian_tra_T *rn;
9586 emxArray_real_T_cartesian_tra_T *e;
9587 emxArray_boolean_T_cartesian__T *x;
9588 emxArray_boolean_T_cartesian__T *x_tmp;
9589 emxArray_boolean_T_cartesian__T *x_tmp_0;
9590 emxArray_boolean_T_cartesian__T *x_0;
9591 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
9592 'l', 'a', 'b', 'l', 'e' };
9593
9594 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
9595
9596 boolean_T guard1 = false;
9597 boolean_T guard2 = false;
9598 boolean_T guard3 = false;
9599 boolean_T exitg1;
9600 boolean_T exitg2;
9601 obj->MaxNumIterationInternal = obj->MaxNumIteration;
9602 obj->MaxTimeInternal = obj->MaxTime;
9603 for (cartesian_trajectory_planner_B.i_h = 0;
9604 cartesian_trajectory_planner_B.i_h < 6;
9605 cartesian_trajectory_planner_B.i_h++) {
9606 obj->SeedInternal[cartesian_trajectory_planner_B.i_h] =
9607 seed[cartesian_trajectory_planner_B.i_h];
9608 }
9609
9610 cartesian_trajectory_planner_B.tol = obj->SolutionTolerance;
9611 obj->TimeObj.StartTime = ctimefun();
9612 DampedBFGSwGradientProjection_s(obj, xSol,
9613 &cartesian_trajectory_planner_B.exitFlag,
9614 &cartesian_trajectory_planner_B.err, &cartesian_trajectory_planner_B.iter);
9615 *solutionInfo_RRAttempts = 0.0;
9616 *solutionInfo_Iterations = cartesian_trajectory_planner_B.iter;
9617 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
9618 cartesian_trajectory_planner_B.exitFlagPrev =
9619 cartesian_trajectory_planner_B.exitFlag;
9620 cartesian_trajec_emxInit_real_T(&newseed, 1);
9621 cartesian_trajec_emxInit_real_T(&qi, 2);
9622 cartesian_trajec_emxInit_real_T(&ub, 1);
9623 cartesian_trajec_emxInit_real_T(&lb, 1);
9624 cartesian_trajec_emxInit_real_T(&rn, 1);
9625 cartesian_trajec_emxInit_real_T(&e, 2);
9626 cartesian_tra_emxInit_boolean_T(&x, 1);
9627 cartesian_tra_emxInit_boolean_T(&x_tmp, 1);
9628 cartesian_tra_emxInit_boolean_T(&x_tmp_0, 1);
9629 cartesian_tra_emxInit_boolean_T(&x_0, 1);
9630 exitg1 = false;
9631 while ((!exitg1) && (obj->RandomRestart && (cartesian_trajectory_planner_B.err
9632 > cartesian_trajectory_planner_B.tol))) {
9633 obj->MaxNumIterationInternal -= cartesian_trajectory_planner_B.iter;
9634 cartesian_trajectory_planner_B.err = ctimefun();
9635 cartesian_trajectory_planner_B.err -= obj->TimeObj.StartTime;
9636 obj->MaxTimeInternal = obj->MaxTime - cartesian_trajectory_planner_B.err;
9637 if (obj->MaxNumIterationInternal <= 0.0) {
9638 cartesian_trajectory_planner_B.exitFlag = IterationLimitExceeded;
9639 }
9640
9641 if ((cartesian_trajectory_planner_B.exitFlag == IterationLimitExceeded) ||
9642 (cartesian_trajectory_planner_B.exitFlag == TimeLimitExceeded)) {
9643 cartesian_trajectory_planner_B.exitFlagPrev =
9644 cartesian_trajectory_planner_B.exitFlag;
9645 exitg1 = true;
9646 } else {
9647 args = obj->ExtraArgs;
9648 obj_0 = args->Robot;
9649 cartesian_trajectory_planner_B.ix = newseed->size[0];
9650 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
9651 cartes_emxEnsureCapacity_real_T(newseed, cartesian_trajectory_planner_B.ix);
9652 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
9653 (obj_0->PositionNumber);
9654 for (cartesian_trajectory_planner_B.ix = 0;
9655 cartesian_trajectory_planner_B.ix < cartesian_trajectory_planner_B.nx;
9656 cartesian_trajectory_planner_B.ix++) {
9657 newseed->data[cartesian_trajectory_planner_B.ix] = 0.0;
9658 }
9659
9660 cartesian_trajectory_planner_B.err = obj_0->NumBodies;
9661 cartesian_trajectory_planner_B.c_n = static_cast<int32_T>
9662 (cartesian_trajectory_planner_B.err) - 1;
9663 for (cartesian_trajectory_planner_B.i_h = 0;
9664 cartesian_trajectory_planner_B.i_h <=
9665 cartesian_trajectory_planner_B.c_n;
9666 cartesian_trajectory_planner_B.i_h++) {
9667 cartesian_trajectory_planner_B.err = obj_0->
9668 PositionDoFMap[cartesian_trajectory_planner_B.i_h];
9669 cartesian_trajectory_planner_B.iter = obj_0->
9670 PositionDoFMap[cartesian_trajectory_planner_B.i_h + 8];
9671 if (cartesian_trajectory_planner_B.err <=
9672 cartesian_trajectory_planner_B.iter) {
9673 obj_1 = obj_0->Bodies[cartesian_trajectory_planner_B.i_h]
9674 ->JointInternal;
9675 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
9676 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
9677 qi->size[0] = 1;
9678 qi->size[1] = 1;
9679 cartes_emxEnsureCapacity_real_T(qi,
9680 cartesian_trajectory_planner_B.ix);
9681 qi->data[0] = (rtNaN);
9682 } else {
9683 cartesian_trajectory_planner_B.nx = obj_1->
9684 PositionLimitsInternal->size[0];
9685 cartesian_trajectory_planner_B.ix = ub->size[0];
9686 ub->size[0] = cartesian_trajectory_planner_B.nx;
9687 cartes_emxEnsureCapacity_real_T(ub,
9688 cartesian_trajectory_planner_B.ix);
9689 for (cartesian_trajectory_planner_B.ix = 0;
9690 cartesian_trajectory_planner_B.ix <
9691 cartesian_trajectory_planner_B.nx;
9692 cartesian_trajectory_planner_B.ix++) {
9693 ub->data[cartesian_trajectory_planner_B.ix] =
9694 obj_1->PositionLimitsInternal->
9695 data[cartesian_trajectory_planner_B.ix +
9696 obj_1->PositionLimitsInternal->size[0]];
9697 }
9698
9699 cartesian_trajectory_planner_B.nx = obj_1->
9700 PositionLimitsInternal->size[0];
9701 cartesian_trajectory_planner_B.ix = lb->size[0];
9702 lb->size[0] = cartesian_trajectory_planner_B.nx;
9703 cartes_emxEnsureCapacity_real_T(lb,
9704 cartesian_trajectory_planner_B.ix);
9705 for (cartesian_trajectory_planner_B.ix = 0;
9706 cartesian_trajectory_planner_B.ix <
9707 cartesian_trajectory_planner_B.nx;
9708 cartesian_trajectory_planner_B.ix++) {
9709 lb->data[cartesian_trajectory_planner_B.ix] =
9710 obj_1->PositionLimitsInternal->
9711 data[cartesian_trajectory_planner_B.ix];
9712 }
9713
9714 cartesian_trajectory_p_isfinite(lb, x_tmp);
9715 cartesian_trajectory_planner_B.y_fd = true;
9716 cartesian_trajectory_planner_B.ix = 0;
9717 exitg2 = false;
9718 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
9719 x_tmp->size[0])) {
9720 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
9721 cartesian_trajectory_planner_B.y_fd = false;
9722 exitg2 = true;
9723 } else {
9724 cartesian_trajectory_planner_B.ix++;
9725 }
9726 }
9727
9728 guard1 = false;
9729 guard2 = false;
9730 guard3 = false;
9731 if (cartesian_trajectory_planner_B.y_fd) {
9732 cartesian_trajectory_p_isfinite(ub, x);
9733 cartesian_trajectory_planner_B.y_fd = true;
9734 cartesian_trajectory_planner_B.ix = 0;
9735 exitg2 = false;
9736 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
9737 x->size[0])) {
9738 if (!x->data[cartesian_trajectory_planner_B.ix]) {
9739 cartesian_trajectory_planner_B.y_fd = false;
9740 exitg2 = true;
9741 } else {
9742 cartesian_trajectory_planner_B.ix++;
9743 }
9744 }
9745
9746 if (cartesian_trajectory_planner_B.y_fd) {
9747 cartesian_trajectory_pla_rand_a(obj_1->PositionNumber, rn);
9748 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
9749 qi->size[0] = lb->size[0];
9750 qi->size[1] = 1;
9751 cartes_emxEnsureCapacity_real_T(qi,
9752 cartesian_trajectory_planner_B.ix);
9753 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
9754 for (cartesian_trajectory_planner_B.ix = 0;
9755 cartesian_trajectory_planner_B.ix <=
9756 cartesian_trajectory_planner_B.nx;
9757 cartesian_trajectory_planner_B.ix++) {
9758 qi->data[cartesian_trajectory_planner_B.ix] = (ub->
9759 data[cartesian_trajectory_planner_B.ix] - lb->
9760 data[cartesian_trajectory_planner_B.ix]) * rn->
9761 data[cartesian_trajectory_planner_B.ix] + lb->
9762 data[cartesian_trajectory_planner_B.ix];
9763 }
9764 } else {
9765 guard3 = true;
9766 }
9767 } else {
9768 guard3 = true;
9769 }
9770
9771 if (guard3) {
9772 cartesian_trajectory_planner_B.y_fd = true;
9773 cartesian_trajectory_planner_B.ix = 0;
9774 exitg2 = false;
9775 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
9776 x_tmp->size[0])) {
9777 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
9778 cartesian_trajectory_planner_B.y_fd = false;
9779 exitg2 = true;
9780 } else {
9781 cartesian_trajectory_planner_B.ix++;
9782 }
9783 }
9784
9785 if (cartesian_trajectory_planner_B.y_fd) {
9786 cartesian_trajectory_p_isfinite(ub, x);
9787 cartesian_trajectory_planner_B.ix = x_0->size[0];
9788 x_0->size[0] = x->size[0];
9789 car_emxEnsureCapacity_boolean_T(x_0,
9790 cartesian_trajectory_planner_B.ix);
9791 cartesian_trajectory_planner_B.nx = x->size[0];
9792 for (cartesian_trajectory_planner_B.ix = 0;
9793 cartesian_trajectory_planner_B.ix <
9794 cartesian_trajectory_planner_B.nx;
9795 cartesian_trajectory_planner_B.ix++) {
9796 x_0->data[cartesian_trajectory_planner_B.ix] = !x->
9797 data[cartesian_trajectory_planner_B.ix];
9798 }
9799
9800 if (cartesian_trajectory_planne_any(x_0)) {
9801 cartesian_trajectory_planner_B.ub[0] = lb->size[0];
9802 cartesian_trajectory_planner_B.ub[1] = 1.0;
9803 cartesian_trajectory_plan_randn
9804 (cartesian_trajectory_planner_B.ub, qi);
9805 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
9806 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
9807 e->size[0] = qi->size[0];
9808 e->size[1] = 1;
9809 cartes_emxEnsureCapacity_real_T(e,
9810 cartesian_trajectory_planner_B.ix);
9811 for (cartesian_trajectory_planner_B.ix = 0;
9812 cartesian_trajectory_planner_B.ix <=
9813 cartesian_trajectory_planner_B.nx;
9814 cartesian_trajectory_planner_B.ix++) {
9815 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
9816 data[cartesian_trajectory_planner_B.ix]);
9817 }
9818
9819 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
9820 qi->size[0] = lb->size[0];
9821 qi->size[1] = 1;
9822 cartes_emxEnsureCapacity_real_T(qi,
9823 cartesian_trajectory_planner_B.ix);
9824 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
9825 for (cartesian_trajectory_planner_B.ix = 0;
9826 cartesian_trajectory_planner_B.ix <=
9827 cartesian_trajectory_planner_B.nx;
9828 cartesian_trajectory_planner_B.ix++) {
9829 qi->data[cartesian_trajectory_planner_B.ix] = lb->
9830 data[cartesian_trajectory_planner_B.ix] + e->
9831 data[cartesian_trajectory_planner_B.ix];
9832 }
9833 } else {
9834 guard2 = true;
9835 }
9836 } else {
9837 guard2 = true;
9838 }
9839 }
9840
9841 if (guard2) {
9842 cartesian_trajectory_planner_B.ix = x_tmp_0->size[0];
9843 x_tmp_0->size[0] = x_tmp->size[0];
9844 car_emxEnsureCapacity_boolean_T(x_tmp_0,
9845 cartesian_trajectory_planner_B.ix);
9846 cartesian_trajectory_planner_B.nx = x_tmp->size[0];
9847 for (cartesian_trajectory_planner_B.ix = 0;
9848 cartesian_trajectory_planner_B.ix <
9849 cartesian_trajectory_planner_B.nx;
9850 cartesian_trajectory_planner_B.ix++) {
9851 x_tmp_0->data[cartesian_trajectory_planner_B.ix] = !x_tmp->
9852 data[cartesian_trajectory_planner_B.ix];
9853 }
9854
9855 if (cartesian_trajectory_planne_any(x_tmp_0)) {
9856 cartesian_trajectory_p_isfinite(ub, x);
9857 cartesian_trajectory_planner_B.y_fd = true;
9858 cartesian_trajectory_planner_B.ix = 0;
9859 exitg2 = false;
9860 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
9861 x->size[0])) {
9862 if (!x->data[cartesian_trajectory_planner_B.ix]) {
9863 cartesian_trajectory_planner_B.y_fd = false;
9864 exitg2 = true;
9865 } else {
9866 cartesian_trajectory_planner_B.ix++;
9867 }
9868 }
9869
9870 if (cartesian_trajectory_planner_B.y_fd) {
9871 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
9872 cartesian_trajectory_planner_B.ub[1] = 1.0;
9873 cartesian_trajectory_plan_randn
9874 (cartesian_trajectory_planner_B.ub, qi);
9875 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
9876 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
9877 e->size[0] = qi->size[0];
9878 e->size[1] = 1;
9879 cartes_emxEnsureCapacity_real_T(e,
9880 cartesian_trajectory_planner_B.ix);
9881 for (cartesian_trajectory_planner_B.ix = 0;
9882 cartesian_trajectory_planner_B.ix <=
9883 cartesian_trajectory_planner_B.nx;
9884 cartesian_trajectory_planner_B.ix++) {
9885 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
9886 data[cartesian_trajectory_planner_B.ix]);
9887 }
9888
9889 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
9890 qi->size[0] = ub->size[0];
9891 qi->size[1] = 1;
9892 cartes_emxEnsureCapacity_real_T(qi,
9893 cartesian_trajectory_planner_B.ix);
9894 cartesian_trajectory_planner_B.nx = ub->size[0] - 1;
9895 for (cartesian_trajectory_planner_B.ix = 0;
9896 cartesian_trajectory_planner_B.ix <=
9897 cartesian_trajectory_planner_B.nx;
9898 cartesian_trajectory_planner_B.ix++) {
9899 qi->data[cartesian_trajectory_planner_B.ix] = ub->
9900 data[cartesian_trajectory_planner_B.ix] - e->
9901 data[cartesian_trajectory_planner_B.ix];
9902 }
9903 } else {
9904 guard1 = true;
9905 }
9906 } else {
9907 guard1 = true;
9908 }
9909 }
9910
9911 if (guard1) {
9912 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
9913 cartesian_trajectory_planner_B.ub[1] = 1.0;
9914 cartesian_trajectory_plan_randn(cartesian_trajectory_planner_B.ub,
9915 qi);
9916 }
9917 }
9918
9919 if (cartesian_trajectory_planner_B.err >
9920 cartesian_trajectory_planner_B.iter) {
9921 cartesian_trajectory_planner_B.nx = 0;
9922 cartesian_trajectory_planner_B.ix = 0;
9923 } else {
9924 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
9925 (cartesian_trajectory_planner_B.err) - 1;
9926 cartesian_trajectory_planner_B.ix = static_cast<int32_T>
9927 (cartesian_trajectory_planner_B.iter);
9928 }
9929
9930 cartesian_trajectory_planner_B.unnamed_idx_1 =
9931 cartesian_trajectory_planner_B.ix -
9932 cartesian_trajectory_planner_B.nx;
9933 for (cartesian_trajectory_planner_B.ix = 0;
9934 cartesian_trajectory_planner_B.ix <
9935 cartesian_trajectory_planner_B.unnamed_idx_1;
9936 cartesian_trajectory_planner_B.ix++) {
9937 newseed->data[cartesian_trajectory_planner_B.nx +
9938 cartesian_trajectory_planner_B.ix] = qi->
9939 data[cartesian_trajectory_planner_B.ix];
9940 }
9941 }
9942 }
9943
9944 for (cartesian_trajectory_planner_B.ix = 0;
9945 cartesian_trajectory_planner_B.ix < 6;
9946 cartesian_trajectory_planner_B.ix++) {
9947 obj->SeedInternal[cartesian_trajectory_planner_B.ix] = newseed->
9948 data[cartesian_trajectory_planner_B.ix];
9949 }
9950
9951 DampedBFGSwGradientProjection_s(obj, cartesian_trajectory_planner_B.c_xSol,
9952 &cartesian_trajectory_planner_B.exitFlag,
9953 &cartesian_trajectory_planner_B.err,
9954 &cartesian_trajectory_planner_B.iter);
9955 if (cartesian_trajectory_planner_B.err < *solutionInfo_Error) {
9956 for (cartesian_trajectory_planner_B.i_h = 0;
9957 cartesian_trajectory_planner_B.i_h < 6;
9958 cartesian_trajectory_planner_B.i_h++) {
9959 xSol[cartesian_trajectory_planner_B.i_h] =
9960 cartesian_trajectory_planner_B.c_xSol[cartesian_trajectory_planner_B.i_h];
9961 }
9962
9963 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
9964 cartesian_trajectory_planner_B.exitFlagPrev =
9965 cartesian_trajectory_planner_B.exitFlag;
9966 }
9967
9968 (*solutionInfo_RRAttempts)++;
9969 *solutionInfo_Iterations += cartesian_trajectory_planner_B.iter;
9970 }
9971 }
9972
9973 cartesian_tra_emxFree_boolean_T(&x_0);
9974 cartesian_tra_emxFree_boolean_T(&x_tmp_0);
9975 cartesian_tra_emxFree_boolean_T(&x_tmp);
9976 cartesian_tra_emxFree_boolean_T(&x);
9977 cartesian_trajec_emxFree_real_T(&e);
9978 cartesian_trajec_emxFree_real_T(&rn);
9979 cartesian_trajec_emxFree_real_T(&lb);
9980 cartesian_trajec_emxFree_real_T(&ub);
9981 cartesian_trajec_emxFree_real_T(&qi);
9982 cartesian_trajec_emxFree_real_T(&newseed);
9983 *solutionInfo_ExitFlag = cartesian_trajectory_planner_B.exitFlagPrev;
9984 if (*solutionInfo_Error < cartesian_trajectory_planner_B.tol) {
9985 solutionInfo_Status_size[0] = 1;
9986 solutionInfo_Status_size[1] = 7;
9987 for (cartesian_trajectory_planner_B.ix = 0;
9988 cartesian_trajectory_planner_B.ix < 7;
9989 cartesian_trajectory_planner_B.ix++) {
9990 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
9991 tmp_0[cartesian_trajectory_planner_B.ix];
9992 }
9993 } else {
9994 solutionInfo_Status_size[0] = 1;
9995 solutionInfo_Status_size[1] = 14;
9996 for (cartesian_trajectory_planner_B.ix = 0;
9997 cartesian_trajectory_planner_B.ix < 14;
9998 cartesian_trajectory_planner_B.ix++) {
9999 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
10000 tmp[cartesian_trajectory_planner_B.ix];
10001 }
10002 }
10003}
10004
10005static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
10006 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
10007 real_T QSol[6])
10008{
10009 emxArray_real_T_cartesian_tra_T *bodyIndices;
10010 emxArray_real_T_cartesian_tra_T *positionIndices;
10011 x_robotics_manip_internal_Rig_T *obj_0;
10012 emxArray_char_T_cartesian_tra_T *endEffectorName;
10013 w_robotics_manip_internal_Rig_T *body;
10014 emxArray_real_T_cartesian_tra_T *positionMap;
10015 emxArray_real_T_cartesian_tra_T *e;
10016 emxArray_int32_T_cartesian_tr_T *h;
10017 emxArray_real_T_cartesian_tra_T *y;
10018 emxArray_char_T_cartesian_tra_T *bname;
10019 emxArray_real_T_cartesian_tra_T *bodyIndices_0;
10020 boolean_T exitg1;
10021 c_inverseKinematics_setPoseGoal(obj, tform, weights);
10022 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <
10023 6; cartesian_trajectory_planner_B.i++) {
10024 QSol[cartesian_trajectory_planner_B.i] =
10025 initialGuess[cartesian_trajectory_planner_B.i];
10026 }
10027
10028 cartesian_trajec_emxInit_char_T(&endEffectorName, 2);
10029 RigidBodyTree_validateConfigu_a(obj->RigidBodyTreeInternal, QSol);
10030 cartes_NLPSolverInterface_solve(obj->Solver, QSol,
10031 cartesian_trajectory_planner_B.qvSolRaw, &cartesian_trajectory_planner_B.bid,
10032 &cartesian_trajectory_planner_B.numPositions,
10033 &cartesian_trajectory_planner_B.ndbl, &cartesian_trajectory_planner_B.apnd,
10034 cartesian_trajectory_planner_B.sol_Status_data,
10035 cartesian_trajectory_planner_B.sol_Status_size);
10036 obj_0 = obj->RigidBodyTreeInternal;
10037 cartesian_trajectory_planner_B.partialTrueCount = endEffectorName->size[0] *
10038 endEffectorName->size[1];
10039 endEffectorName->size[0] = 1;
10040 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
10041 cartes_emxEnsureCapacity_char_T(endEffectorName,
10042 cartesian_trajectory_planner_B.partialTrueCount);
10043 cartesian_trajectory_planner_B.loop_ub = obj->Solver->ExtraArgs->
10044 BodyName->size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;
10045 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10046 cartesian_trajectory_planner_B.partialTrueCount <=
10047 cartesian_trajectory_planner_B.loop_ub;
10048 cartesian_trajectory_planner_B.partialTrueCount++) {
10049 endEffectorName->data[cartesian_trajectory_planner_B.partialTrueCount] =
10050 obj->Solver->ExtraArgs->BodyName->
10051 data[cartesian_trajectory_planner_B.partialTrueCount];
10052 }
10053
10054 cartesian_trajec_emxInit_real_T(&bodyIndices, 1);
10055 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10056 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
10057 cartes_emxEnsureCapacity_real_T(bodyIndices,
10058 cartesian_trajectory_planner_B.partialTrueCount);
10059 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);
10060 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10061 cartesian_trajectory_planner_B.partialTrueCount <
10062 cartesian_trajectory_planner_B.loop_ub;
10063 cartesian_trajectory_planner_B.partialTrueCount++) {
10064 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
10065 }
10066
10067 cartesian_trajec_emxInit_char_T(&bname, 2);
10068 cartesian_trajectory_planner_B.bid = -1.0;
10069 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] * bname->
10070 size[1];
10071 bname->size[0] = 1;
10072 bname->size[1] = obj_0->Base.NameInternal->size[1];
10073 cartes_emxEnsureCapacity_char_T(bname,
10074 cartesian_trajectory_planner_B.partialTrueCount);
10075 cartesian_trajectory_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *
10076 obj_0->Base.NameInternal->size[1] - 1;
10077 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10078 cartesian_trajectory_planner_B.partialTrueCount <=
10079 cartesian_trajectory_planner_B.loop_ub;
10080 cartesian_trajectory_planner_B.partialTrueCount++) {
10081 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
10082 obj_0->Base.NameInternal->
10083 data[cartesian_trajectory_planner_B.partialTrueCount];
10084 }
10085
10086 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
10087 cartesian_trajectory_planner_B.bid = 0.0;
10088 } else {
10089 cartesian_trajectory_planner_B.numPositions = obj_0->NumBodies;
10090 cartesian_trajectory_planner_B.i = 0;
10091 exitg1 = false;
10092 while ((!exitg1) && (cartesian_trajectory_planner_B.i <= static_cast<int32_T>
10093 (cartesian_trajectory_planner_B.numPositions) - 1)) {
10094 body = obj_0->Bodies[cartesian_trajectory_planner_B.i];
10095 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] *
10096 bname->size[1];
10097 bname->size[0] = 1;
10098 bname->size[1] = body->NameInternal->size[1];
10099 cartes_emxEnsureCapacity_char_T(bname,
10100 cartesian_trajectory_planner_B.partialTrueCount);
10101 cartesian_trajectory_planner_B.loop_ub = body->NameInternal->size[0] *
10102 body->NameInternal->size[1] - 1;
10103 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10104 cartesian_trajectory_planner_B.partialTrueCount <=
10105 cartesian_trajectory_planner_B.loop_ub;
10106 cartesian_trajectory_planner_B.partialTrueCount++) {
10107 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
10108 body->NameInternal->
10109 data[cartesian_trajectory_planner_B.partialTrueCount];
10110 }
10111
10112 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
10113 cartesian_trajectory_planner_B.bid = static_cast<real_T>
10114 (cartesian_trajectory_planner_B.i) + 1.0;
10115 exitg1 = true;
10116 } else {
10117 cartesian_trajectory_planner_B.i++;
10118 }
10119 }
10120 }
10121
10122 cartesian_trajec_emxFree_char_T(&bname);
10123 cartesian_trajec_emxFree_char_T(&endEffectorName);
10124 if (cartesian_trajectory_planner_B.bid == 0.0) {
10125 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10126 bodyIndices->size[0] = 1;
10127 cartes_emxEnsureCapacity_real_T(bodyIndices,
10128 cartesian_trajectory_planner_B.partialTrueCount);
10129 bodyIndices->data[0] = 0.0;
10130 } else {
10131 body = obj_0->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
10132 - 1];
10133 cartesian_trajectory_planner_B.bid = 1.0;
10134 while (body->ParentIndex != 0.0) {
10135 bodyIndices->data[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
10136 - 1] = body->Index;
10137 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
10138 cartesian_trajectory_planner_B.bid++;
10139 }
10140
10141 if (1.0 > cartesian_trajectory_planner_B.bid - 1.0) {
10142 cartesian_trajectory_planner_B.c_i = -1;
10143 } else {
10144 cartesian_trajectory_planner_B.c_i = static_cast<int32_T>
10145 (cartesian_trajectory_planner_B.bid - 1.0) - 1;
10146 }
10147
10148 cartesian_trajec_emxInit_real_T(&bodyIndices_0, 1);
10149 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices_0->size[0];
10150 bodyIndices_0->size[0] = cartesian_trajectory_planner_B.c_i + 3;
10151 cartes_emxEnsureCapacity_real_T(bodyIndices_0,
10152 cartesian_trajectory_planner_B.partialTrueCount);
10153 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10154 cartesian_trajectory_planner_B.partialTrueCount <=
10155 cartesian_trajectory_planner_B.c_i;
10156 cartesian_trajectory_planner_B.partialTrueCount++) {
10157 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount] =
10158 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount];
10159 }
10160
10161 bodyIndices_0->data[cartesian_trajectory_planner_B.c_i + 1] = body->Index;
10162 bodyIndices_0->data[cartesian_trajectory_planner_B.c_i + 2] = 0.0;
10163 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10164 bodyIndices->size[0] = bodyIndices_0->size[0];
10165 cartes_emxEnsureCapacity_real_T(bodyIndices,
10166 cartesian_trajectory_planner_B.partialTrueCount);
10167 cartesian_trajectory_planner_B.loop_ub = bodyIndices_0->size[0];
10168 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10169 cartesian_trajectory_planner_B.partialTrueCount <
10170 cartesian_trajectory_planner_B.loop_ub;
10171 cartesian_trajectory_planner_B.partialTrueCount++) {
10172 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] =
10173 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount];
10174 }
10175
10176 cartesian_trajec_emxFree_real_T(&bodyIndices_0);
10177 }
10178
10179 obj_0 = obj->RigidBodyTreeInternal;
10180 cartesian_trajectory_planner_B.c_i = bodyIndices->size[0] - 1;
10181 cartesian_trajectory_planner_B.loop_ub = 0;
10182 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10183 cartesian_trajectory_planner_B.c_i; cartesian_trajectory_planner_B.i++) {
10184 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
10185 cartesian_trajectory_planner_B.loop_ub++;
10186 }
10187 }
10188
10189 cartesian_traje_emxInit_int32_T(&h, 1);
10190 cartesian_trajectory_planner_B.partialTrueCount = h->size[0];
10191 h->size[0] = cartesian_trajectory_planner_B.loop_ub;
10192 carte_emxEnsureCapacity_int32_T(h,
10193 cartesian_trajectory_planner_B.partialTrueCount);
10194 cartesian_trajectory_planner_B.partialTrueCount = 0;
10195 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10196 cartesian_trajectory_planner_B.c_i; cartesian_trajectory_planner_B.i++) {
10197 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
10198 h->data[cartesian_trajectory_planner_B.partialTrueCount] =
10199 cartesian_trajectory_planner_B.i + 1;
10200 cartesian_trajectory_planner_B.partialTrueCount++;
10201 }
10202 }
10203
10204 cartesian_trajec_emxInit_real_T(&positionMap, 2);
10205 cartesian_trajectory_planner_B.partialTrueCount = positionMap->size[0] *
10206 positionMap->size[1];
10207 positionMap->size[0] = h->size[0];
10208 positionMap->size[1] = 2;
10209 cartes_emxEnsureCapacity_real_T(positionMap,
10210 cartesian_trajectory_planner_B.partialTrueCount);
10211 cartesian_trajectory_planner_B.loop_ub = h->size[0];
10212 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10213 cartesian_trajectory_planner_B.partialTrueCount <
10214 cartesian_trajectory_planner_B.loop_ub;
10215 cartesian_trajectory_planner_B.partialTrueCount++) {
10216 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount] =
10217 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
10218 data[cartesian_trajectory_planner_B.partialTrueCount] - 1]) - 1];
10219 }
10220
10221 cartesian_trajectory_planner_B.loop_ub = h->size[0];
10222 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10223 cartesian_trajectory_planner_B.partialTrueCount <
10224 cartesian_trajectory_planner_B.loop_ub;
10225 cartesian_trajectory_planner_B.partialTrueCount++) {
10226 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount +
10227 positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>
10228 (bodyIndices->data[h->data[cartesian_trajectory_planner_B.partialTrueCount]
10229 - 1]) + 7];
10230 }
10231
10232 cartesian_traje_emxFree_int32_T(&h);
10233 cartesian_trajec_emxFree_real_T(&bodyIndices);
10234 cartesian_trajec_emxInit_real_T(&positionIndices, 2);
10235 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
10236 positionIndices->size[1];
10237 positionIndices->size[0] = 1;
10238 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
10239 cartes_emxEnsureCapacity_real_T(positionIndices,
10240 cartesian_trajectory_planner_B.partialTrueCount);
10241 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>
10242 (obj_0->PositionNumber) - 1;
10243 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10244 cartesian_trajectory_planner_B.partialTrueCount <=
10245 cartesian_trajectory_planner_B.loop_ub;
10246 cartesian_trajectory_planner_B.partialTrueCount++) {
10247 positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
10248 }
10249
10250 cartesian_trajectory_planner_B.bid = 0.0;
10251 cartesian_trajectory_planner_B.c_i = positionMap->size[0] - 1;
10252 cartesian_trajec_emxInit_real_T(&e, 2);
10253 cartesian_trajec_emxInit_real_T(&y, 2);
10254 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10255 cartesian_trajectory_planner_B.c_i; cartesian_trajectory_planner_B.i++) {
10256 cartesian_trajectory_planner_B.numPositions = (positionMap->
10257 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10258 positionMap->data[cartesian_trajectory_planner_B.i]) + 1.0;
10259 if (cartesian_trajectory_planner_B.numPositions > 0.0) {
10260 if (cartesian_trajectory_planner_B.numPositions < 1.0) {
10261 y->size[0] = 1;
10262 y->size[1] = 0;
10263 } else if (rtIsInf(cartesian_trajectory_planner_B.numPositions) && (1.0 ==
10264 cartesian_trajectory_planner_B.numPositions)) {
10265 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
10266 y->size[0] = 1;
10267 y->size[1] = 1;
10268 cartes_emxEnsureCapacity_real_T(y,
10269 cartesian_trajectory_planner_B.partialTrueCount);
10270 y->data[0] = (rtNaN);
10271 } else {
10272 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
10273 y->size[0] = 1;
10274 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
10275 (cartesian_trajectory_planner_B.numPositions - 1.0));
10276 y->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
10277 cartes_emxEnsureCapacity_real_T(y,
10278 cartesian_trajectory_planner_B.partialTrueCount);
10279 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10280 cartesian_trajectory_planner_B.partialTrueCount <=
10281 cartesian_trajectory_planner_B.loop_ub;
10282 cartesian_trajectory_planner_B.partialTrueCount++) {
10283 y->data[cartesian_trajectory_planner_B.partialTrueCount] =
10284 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount)
10285 + 1.0;
10286 }
10287 }
10288
10289 if (rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i]) ||
10290 rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i +
10291 positionMap->size[0]])) {
10292 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10293 e->size[0] = 1;
10294 e->size[1] = 1;
10295 cartes_emxEnsureCapacity_real_T(e,
10296 cartesian_trajectory_planner_B.partialTrueCount);
10297 e->data[0] = (rtNaN);
10298 } else if (positionMap->data[cartesian_trajectory_planner_B.i +
10299 positionMap->size[0]] < positionMap->
10300 data[cartesian_trajectory_planner_B.i]) {
10301 e->size[0] = 1;
10302 e->size[1] = 0;
10303 } else if ((rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i]) ||
10304 rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i +
10305 positionMap->size[0]])) && (positionMap->
10306 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] ==
10307 positionMap->data[cartesian_trajectory_planner_B.i])) {
10308 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10309 e->size[0] = 1;
10310 e->size[1] = 1;
10311 cartes_emxEnsureCapacity_real_T(e,
10312 cartesian_trajectory_planner_B.partialTrueCount);
10313 e->data[0] = (rtNaN);
10314 } else if (floor(positionMap->data[cartesian_trajectory_planner_B.i]) ==
10315 positionMap->data[cartesian_trajectory_planner_B.i]) {
10316 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10317 e->size[0] = 1;
10318 e->size[1] = static_cast<int32_T>(floor(positionMap->
10319 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10320 positionMap->data[cartesian_trajectory_planner_B.i])) + 1;
10321 cartes_emxEnsureCapacity_real_T(e,
10322 cartesian_trajectory_planner_B.partialTrueCount);
10323 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
10324 (positionMap->data[cartesian_trajectory_planner_B.i +
10325 positionMap->size[0]] - positionMap->
10326 data[cartesian_trajectory_planner_B.i]));
10327 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10328 cartesian_trajectory_planner_B.partialTrueCount <=
10329 cartesian_trajectory_planner_B.loop_ub;
10330 cartesian_trajectory_planner_B.partialTrueCount++) {
10331 e->data[cartesian_trajectory_planner_B.partialTrueCount] =
10332 positionMap->data[cartesian_trajectory_planner_B.i] +
10333 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount);
10334 }
10335 } else {
10336 cartesian_trajectory_planner_B.ndbl = floor((positionMap->
10337 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10338 positionMap->data[cartesian_trajectory_planner_B.i]) + 0.5);
10339 cartesian_trajectory_planner_B.apnd = positionMap->
10340 data[cartesian_trajectory_planner_B.i] +
10341 cartesian_trajectory_planner_B.ndbl;
10342 cartesian_trajectory_planner_B.cdiff =
10343 cartesian_trajectory_planner_B.apnd - positionMap->
10344 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
10345 cartesian_trajectory_planner_B.u0 = fabs(positionMap->
10346 data[cartesian_trajectory_planner_B.i]);
10347 cartesian_trajectory_planner_B.u1 = fabs(positionMap->
10348 data[cartesian_trajectory_planner_B.i + positionMap->size[0]]);
10349 if ((cartesian_trajectory_planner_B.u0 >
10350 cartesian_trajectory_planner_B.u1) || rtIsNaN
10351 (cartesian_trajectory_planner_B.u1)) {
10352 cartesian_trajectory_planner_B.u1 = cartesian_trajectory_planner_B.u0;
10353 }
10354
10355 if (fabs(cartesian_trajectory_planner_B.cdiff) < 4.4408920985006262E-16 *
10356 cartesian_trajectory_planner_B.u1) {
10357 cartesian_trajectory_planner_B.ndbl++;
10358 cartesian_trajectory_planner_B.apnd = positionMap->
10359 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
10360 } else if (cartesian_trajectory_planner_B.cdiff > 0.0) {
10361 cartesian_trajectory_planner_B.apnd =
10362 (cartesian_trajectory_planner_B.ndbl - 1.0) + positionMap->
10363 data[cartesian_trajectory_planner_B.i];
10364 } else {
10365 cartesian_trajectory_planner_B.ndbl++;
10366 }
10367
10368 if (cartesian_trajectory_planner_B.ndbl >= 0.0) {
10369 cartesian_trajectory_planner_B.partialTrueCount = static_cast<int32_T>
10370 (cartesian_trajectory_planner_B.ndbl);
10371 } else {
10372 cartesian_trajectory_planner_B.partialTrueCount = 0;
10373 }
10374
10375 cartesian_trajectory_planner_B.loop_ub =
10376 cartesian_trajectory_planner_B.partialTrueCount - 1;
10377 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10378 e->size[0] = 1;
10379 e->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
10380 cartes_emxEnsureCapacity_real_T(e,
10381 cartesian_trajectory_planner_B.partialTrueCount);
10382 if (cartesian_trajectory_planner_B.loop_ub + 1 > 0) {
10383 e->data[0] = positionMap->data[cartesian_trajectory_planner_B.i];
10384 if (cartesian_trajectory_planner_B.loop_ub + 1 > 1) {
10385 e->data[cartesian_trajectory_planner_B.loop_ub] =
10386 cartesian_trajectory_planner_B.apnd;
10387 cartesian_trajectory_planner_B.nm1d2 =
10388 ((cartesian_trajectory_planner_B.loop_ub < 0) +
10389 cartesian_trajectory_planner_B.loop_ub) >> 1;
10390 cartesian_trajectory_planner_B.c_c =
10391 cartesian_trajectory_planner_B.nm1d2 - 2;
10392 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10393 cartesian_trajectory_planner_B.partialTrueCount <=
10394 cartesian_trajectory_planner_B.c_c;
10395 cartesian_trajectory_planner_B.partialTrueCount++) {
10396 cartesian_trajectory_planner_B.k_f4 =
10397 cartesian_trajectory_planner_B.partialTrueCount + 1;
10398 e->data[cartesian_trajectory_planner_B.k_f4] = positionMap->
10399 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
10400 (cartesian_trajectory_planner_B.k_f4);
10401 e->data[cartesian_trajectory_planner_B.loop_ub -
10402 cartesian_trajectory_planner_B.k_f4] =
10403 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
10404 (cartesian_trajectory_planner_B.k_f4);
10405 }
10406
10407 if (cartesian_trajectory_planner_B.nm1d2 << 1 ==
10408 cartesian_trajectory_planner_B.loop_ub) {
10409 e->data[cartesian_trajectory_planner_B.nm1d2] = (positionMap->
10410 data[cartesian_trajectory_planner_B.i] +
10411 cartesian_trajectory_planner_B.apnd) / 2.0;
10412 } else {
10413 e->data[cartesian_trajectory_planner_B.nm1d2] = positionMap->
10414 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
10415 (cartesian_trajectory_planner_B.nm1d2);
10416 e->data[cartesian_trajectory_planner_B.nm1d2 + 1] =
10417 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
10418 (cartesian_trajectory_planner_B.nm1d2);
10419 }
10420 }
10421 }
10422 }
10423
10424 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10425 cartesian_trajectory_planner_B.loop_ub =
10426 cartesian_trajectory_planner_B.partialTrueCount - 1;
10427 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10428 cartesian_trajectory_planner_B.partialTrueCount <=
10429 cartesian_trajectory_planner_B.loop_ub;
10430 cartesian_trajectory_planner_B.partialTrueCount++) {
10431 positionIndices->data[static_cast<int32_T>
10432 (cartesian_trajectory_planner_B.bid + y->
10433 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] = e->
10434 data[cartesian_trajectory_planner_B.partialTrueCount];
10435 }
10436
10437 cartesian_trajectory_planner_B.bid +=
10438 cartesian_trajectory_planner_B.numPositions;
10439 }
10440 }
10441
10442 cartesian_trajec_emxFree_real_T(&y);
10443 cartesian_trajec_emxFree_real_T(&e);
10444 cartesian_trajec_emxFree_real_T(&positionMap);
10445 if (1.0 > cartesian_trajectory_planner_B.bid) {
10446 positionIndices->size[1] = 0;
10447 } else {
10448 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
10449 positionIndices->size[1];
10450 positionIndices->size[1] = static_cast<int32_T>
10451 (cartesian_trajectory_planner_B.bid);
10452 cartes_emxEnsureCapacity_real_T(positionIndices,
10453 cartesian_trajectory_planner_B.partialTrueCount);
10454 }
10455
10456 cartesian_trajectory_planner_B.loop_ub = positionIndices->size[0] *
10457 positionIndices->size[1];
10458 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10459 cartesian_trajectory_planner_B.partialTrueCount <
10460 cartesian_trajectory_planner_B.loop_ub;
10461 cartesian_trajectory_planner_B.partialTrueCount++) {
10462 QSol[static_cast<int32_T>(positionIndices->
10463 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] =
10464 cartesian_trajectory_planner_B.qvSolRaw[static_cast<int32_T>
10465 (positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount]) -
10466 1];
10467 }
10468
10469 cartesian_trajec_emxFree_real_T(&positionIndices);
10470}
10471
10472static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10473 *emxArray, int32_T oldNumel)
10474{
10475 int32_T newNumel;
10476 int32_T i;
10477 void *newData;
10478 if (oldNumel < 0) {
10479 oldNumel = 0;
10480 }
10481
10482 newNumel = 1;
10483 for (i = 0; i < emxArray->numDimensions; i++) {
10484 newNumel *= emxArray->size[i];
10485 }
10486
10487 if (newNumel > emxArray->allocatedSize) {
10488 i = emxArray->allocatedSize;
10489 if (i < 16) {
10490 i = 16;
10491 }
10492
10493 while (i < newNumel) {
10494 if (i > 1073741823) {
10495 i = MAX_int32_T;
10496 } else {
10497 i <<= 1;
10498 }
10499 }
10500
10501 newData = calloc(static_cast<uint32_T>(i), sizeof
10502 (f_cell_wrap_cartesian_traject_T));
10503 if (emxArray->data != NULL) {
10504 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_traject_T)
10505 * oldNumel);
10506 if (emxArray->canFreeData) {
10507 free(emxArray->data);
10508 }
10509 }
10510
10511 emxArray->data = (f_cell_wrap_cartesian_traject_T *)newData;
10512 emxArray->allocatedSize = i;
10513 emxArray->canFreeData = true;
10514 }
10515}
10516
10517static void ca_rigidBodyJoint_get_JointAxis(const
10518 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3])
10519{
10520 emxArray_char_T_cartesian_tra_T *a;
10521 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10522
10523 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
10524
10525 boolean_T guard1 = false;
10526 int32_T exitg1;
10527 cartesian_trajec_emxInit_char_T(&a, 2);
10528 cartesian_trajectory_planner_B.b_kstr_j5 = a->size[0] * a->size[1];
10529 a->size[0] = 1;
10530 a->size[1] = obj->Type->size[1];
10531 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_j5);
10532 cartesian_trajectory_planner_B.loop_ub_a3 = obj->Type->size[0] * obj->
10533 Type->size[1] - 1;
10534 for (cartesian_trajectory_planner_B.b_kstr_j5 = 0;
10535 cartesian_trajectory_planner_B.b_kstr_j5 <=
10536 cartesian_trajectory_planner_B.loop_ub_a3;
10537 cartesian_trajectory_planner_B.b_kstr_j5++) {
10538 a->data[cartesian_trajectory_planner_B.b_kstr_j5] = obj->Type->
10539 data[cartesian_trajectory_planner_B.b_kstr_j5];
10540 }
10541
10542 for (cartesian_trajectory_planner_B.b_kstr_j5 = 0;
10543 cartesian_trajectory_planner_B.b_kstr_j5 < 8;
10544 cartesian_trajectory_planner_B.b_kstr_j5++) {
10545 cartesian_trajectory_planner_B.b_g4[cartesian_trajectory_planner_B.b_kstr_j5]
10546 = tmp[cartesian_trajectory_planner_B.b_kstr_j5];
10547 }
10548
10549 cartesian_trajectory_planner_B.b_bool_aq = false;
10550 if (a->size[1] == 8) {
10551 cartesian_trajectory_planner_B.b_kstr_j5 = 1;
10552 do {
10553 exitg1 = 0;
10554 if (cartesian_trajectory_planner_B.b_kstr_j5 - 1 < 8) {
10555 cartesian_trajectory_planner_B.loop_ub_a3 =
10556 cartesian_trajectory_planner_B.b_kstr_j5 - 1;
10557 if (a->data[cartesian_trajectory_planner_B.loop_ub_a3] !=
10558 cartesian_trajectory_planner_B.b_g4[cartesian_trajectory_planner_B.loop_ub_a3])
10559 {
10560 exitg1 = 1;
10561 } else {
10562 cartesian_trajectory_planner_B.b_kstr_j5++;
10563 }
10564 } else {
10565 cartesian_trajectory_planner_B.b_bool_aq = true;
10566 exitg1 = 1;
10567 }
10568 } while (exitg1 == 0);
10569 }
10570
10571 guard1 = false;
10572 if (cartesian_trajectory_planner_B.b_bool_aq) {
10573 guard1 = true;
10574 } else {
10575 cartesian_trajectory_planner_B.b_kstr_j5 = a->size[0] * a->size[1];
10576 a->size[0] = 1;
10577 a->size[1] = obj->Type->size[1];
10578 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_j5);
10579 cartesian_trajectory_planner_B.loop_ub_a3 = obj->Type->size[0] * obj->
10580 Type->size[1] - 1;
10581 for (cartesian_trajectory_planner_B.b_kstr_j5 = 0;
10582 cartesian_trajectory_planner_B.b_kstr_j5 <=
10583 cartesian_trajectory_planner_B.loop_ub_a3;
10584 cartesian_trajectory_planner_B.b_kstr_j5++) {
10585 a->data[cartesian_trajectory_planner_B.b_kstr_j5] = obj->Type->
10586 data[cartesian_trajectory_planner_B.b_kstr_j5];
10587 }
10588
10589 for (cartesian_trajectory_planner_B.b_kstr_j5 = 0;
10590 cartesian_trajectory_planner_B.b_kstr_j5 < 9;
10591 cartesian_trajectory_planner_B.b_kstr_j5++) {
10592 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.b_kstr_j5]
10593 = tmp_0[cartesian_trajectory_planner_B.b_kstr_j5];
10594 }
10595
10596 cartesian_trajectory_planner_B.b_bool_aq = false;
10597 if (a->size[1] == 9) {
10598 cartesian_trajectory_planner_B.b_kstr_j5 = 1;
10599 do {
10600 exitg1 = 0;
10601 if (cartesian_trajectory_planner_B.b_kstr_j5 - 1 < 9) {
10602 cartesian_trajectory_planner_B.loop_ub_a3 =
10603 cartesian_trajectory_planner_B.b_kstr_j5 - 1;
10604 if (a->data[cartesian_trajectory_planner_B.loop_ub_a3] !=
10605 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.loop_ub_a3])
10606 {
10607 exitg1 = 1;
10608 } else {
10609 cartesian_trajectory_planner_B.b_kstr_j5++;
10610 }
10611 } else {
10612 cartesian_trajectory_planner_B.b_bool_aq = true;
10613 exitg1 = 1;
10614 }
10615 } while (exitg1 == 0);
10616 }
10617
10618 if (cartesian_trajectory_planner_B.b_bool_aq) {
10619 guard1 = true;
10620 } else {
10621 ax[0] = (rtNaN);
10622 ax[1] = (rtNaN);
10623 ax[2] = (rtNaN);
10624 }
10625 }
10626
10627 if (guard1) {
10628 ax[0] = obj->JointAxisInternal[0];
10629 ax[1] = obj->JointAxisInternal[1];
10630 ax[2] = obj->JointAxisInternal[2];
10631 }
10632
10633 cartesian_trajec_emxFree_char_T(&a);
10634}
10635
10636static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
10637 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
10638{
10639 n_robotics_manip_internal_Rig_T *body;
10640 emxArray_char_T_cartesian_tra_T *switch_expression;
10641 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
10642 };
10643
10644 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
10645
10646 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10647
10648 int32_T exitg1;
10649 cartesian_trajectory_planner_B.n = obj->NumBodies;
10650 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10651 cartesian_trajectory_planner_B.b_kstr_d < 16;
10652 cartesian_trajectory_planner_B.b_kstr_d++) {
10653 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_d]
10654 = tmp[cartesian_trajectory_planner_B.b_kstr_d];
10655 }
10656
10657 cartesian_trajectory_planner_B.b_kstr_d = Ttree->size[0] * Ttree->size[1];
10658 Ttree->size[0] = 1;
10659 cartesian_trajectory_planner_B.e_kb = static_cast<int32_T>
10660 (cartesian_trajectory_planner_B.n);
10661 Ttree->size[1] = cartesian_trajectory_planner_B.e_kb;
10662 c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_trajectory_planner_B.b_kstr_d);
10663 if (cartesian_trajectory_planner_B.e_kb != 0) {
10664 cartesian_trajectory_planner_B.ntilecols =
10665 cartesian_trajectory_planner_B.e_kb - 1;
10666 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
10667 memcpy(&cartesian_trajectory_planner_B.expl_temp.f1[0],
10668 &cartesian_trajectory_planner_B.c_f1[0], sizeof(real_T) << 4U);
10669 }
10670
10671 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
10672 cartesian_trajectory_planner_B.b_jtilecol <=
10673 cartesian_trajectory_planner_B.ntilecols;
10674 cartesian_trajectory_planner_B.b_jtilecol++) {
10675 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol] =
10676 cartesian_trajectory_planner_B.expl_temp;
10677 }
10678 }
10679
10680 cartesian_trajectory_planner_B.k = 1.0;
10681 cartesian_trajectory_planner_B.ntilecols = static_cast<int32_T>
10682 (cartesian_trajectory_planner_B.n) - 1;
10683 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
10684 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
10685 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10686 cartesian_trajectory_planner_B.b_kstr_d < 5;
10687 cartesian_trajectory_planner_B.b_kstr_d++) {
10688 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.b_kstr_d]
10689 = tmp_0[cartesian_trajectory_planner_B.b_kstr_d];
10690 }
10691 }
10692
10693 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
10694 cartesian_trajectory_planner_B.b_jtilecol <=
10695 cartesian_trajectory_planner_B.ntilecols;
10696 cartesian_trajectory_planner_B.b_jtilecol++) {
10697 body = obj->Bodies[cartesian_trajectory_planner_B.b_jtilecol];
10698 cartesian_trajectory_planner_B.n = body->JointInternal.PositionNumber;
10699 cartesian_trajectory_planner_B.n += cartesian_trajectory_planner_B.k;
10700 if (cartesian_trajectory_planner_B.k > cartesian_trajectory_planner_B.n -
10701 1.0) {
10702 cartesian_trajectory_planner_B.e_kb = 0;
10703 cartesian_trajectory_planner_B.d_l = 0;
10704 } else {
10705 cartesian_trajectory_planner_B.e_kb = static_cast<int32_T>
10706 (cartesian_trajectory_planner_B.k) - 1;
10707 cartesian_trajectory_planner_B.d_l = static_cast<int32_T>
10708 (cartesian_trajectory_planner_B.n - 1.0);
10709 }
10710
10711 cartesian_trajectory_planner_B.b_kstr_d = switch_expression->size[0] *
10712 switch_expression->size[1];
10713 switch_expression->size[0] = 1;
10714 switch_expression->size[1] = body->JointInternal.Type->size[1];
10715 cartes_emxEnsureCapacity_char_T(switch_expression,
10716 cartesian_trajectory_planner_B.b_kstr_d);
10717 cartesian_trajectory_planner_B.loop_ub_n = body->JointInternal.Type->size[0]
10718 * body->JointInternal.Type->size[1] - 1;
10719 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10720 cartesian_trajectory_planner_B.b_kstr_d <=
10721 cartesian_trajectory_planner_B.loop_ub_n;
10722 cartesian_trajectory_planner_B.b_kstr_d++) {
10723 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_d] =
10724 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_d];
10725 }
10726
10727 cartesian_trajectory_planner_B.b_bool_an = false;
10728 if (switch_expression->size[1] == 5) {
10729 cartesian_trajectory_planner_B.b_kstr_d = 1;
10730 do {
10731 exitg1 = 0;
10732 if (cartesian_trajectory_planner_B.b_kstr_d - 1 < 5) {
10733 cartesian_trajectory_planner_B.loop_ub_n =
10734 cartesian_trajectory_planner_B.b_kstr_d - 1;
10735 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_n]
10736 !=
10737 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.loop_ub_n])
10738 {
10739 exitg1 = 1;
10740 } else {
10741 cartesian_trajectory_planner_B.b_kstr_d++;
10742 }
10743 } else {
10744 cartesian_trajectory_planner_B.b_bool_an = true;
10745 exitg1 = 1;
10746 }
10747 } while (exitg1 == 0);
10748 }
10749
10750 if (cartesian_trajectory_planner_B.b_bool_an) {
10751 cartesian_trajectory_planner_B.b_kstr_d = 0;
10752 } else {
10753 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10754 cartesian_trajectory_planner_B.b_kstr_d < 8;
10755 cartesian_trajectory_planner_B.b_kstr_d++) {
10756 cartesian_trajectory_planner_B.b_dz[cartesian_trajectory_planner_B.b_kstr_d]
10757 = tmp_1[cartesian_trajectory_planner_B.b_kstr_d];
10758 }
10759
10760 cartesian_trajectory_planner_B.b_bool_an = false;
10761 if (switch_expression->size[1] == 8) {
10762 cartesian_trajectory_planner_B.b_kstr_d = 1;
10763 do {
10764 exitg1 = 0;
10765 if (cartesian_trajectory_planner_B.b_kstr_d - 1 < 8) {
10766 cartesian_trajectory_planner_B.loop_ub_n =
10767 cartesian_trajectory_planner_B.b_kstr_d - 1;
10768 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_n]
10769 !=
10770 cartesian_trajectory_planner_B.b_dz[cartesian_trajectory_planner_B.loop_ub_n])
10771 {
10772 exitg1 = 1;
10773 } else {
10774 cartesian_trajectory_planner_B.b_kstr_d++;
10775 }
10776 } else {
10777 cartesian_trajectory_planner_B.b_bool_an = true;
10778 exitg1 = 1;
10779 }
10780 } while (exitg1 == 0);
10781 }
10782
10783 if (cartesian_trajectory_planner_B.b_bool_an) {
10784 cartesian_trajectory_planner_B.b_kstr_d = 1;
10785 } else {
10786 cartesian_trajectory_planner_B.b_kstr_d = -1;
10787 }
10788 }
10789
10790 switch (cartesian_trajectory_planner_B.b_kstr_d) {
10791 case 0:
10792 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10793 cartesian_trajectory_planner_B.c_f1[0] = 1.0;
10794 cartesian_trajectory_planner_B.c_f1[5] = 1.0;
10795 cartesian_trajectory_planner_B.c_f1[10] = 1.0;
10796 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
10797 break;
10798
10799 case 1:
10800 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
10801 cartesian_trajectory_planner_B.v_g);
10802 cartesian_trajectory_planner_B.d_l -= cartesian_trajectory_planner_B.e_kb;
10803 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10804 cartesian_trajectory_planner_B.b_kstr_d <
10805 cartesian_trajectory_planner_B.d_l;
10806 cartesian_trajectory_planner_B.b_kstr_d++) {
10807 cartesian_trajectory_planner_B.e_data[cartesian_trajectory_planner_B.b_kstr_d]
10808 = cartesian_trajectory_planner_B.e_kb +
10809 cartesian_trajectory_planner_B.b_kstr_d;
10810 }
10811
10812 cartesian_trajectory_planner_B.result_data_p[0] =
10813 cartesian_trajectory_planner_B.v_g[0];
10814 cartesian_trajectory_planner_B.result_data_p[1] =
10815 cartesian_trajectory_planner_B.v_g[1];
10816 cartesian_trajectory_planner_B.result_data_p[2] =
10817 cartesian_trajectory_planner_B.v_g[2];
10818 if (0 <= (cartesian_trajectory_planner_B.d_l != 0) - 1) {
10819 cartesian_trajectory_planner_B.result_data_p[3] =
10820 qvec[cartesian_trajectory_planner_B.e_data[0]];
10821 }
10822
10823 cartesian_trajectory_planner_B.k = 1.0 / sqrt
10824 ((cartesian_trajectory_planner_B.result_data_p[0] *
10825 cartesian_trajectory_planner_B.result_data_p[0] +
10826 cartesian_trajectory_planner_B.result_data_p[1] *
10827 cartesian_trajectory_planner_B.result_data_p[1]) +
10828 cartesian_trajectory_planner_B.result_data_p[2] *
10829 cartesian_trajectory_planner_B.result_data_p[2]);
10830 cartesian_trajectory_planner_B.v_g[0] =
10831 cartesian_trajectory_planner_B.result_data_p[0] *
10832 cartesian_trajectory_planner_B.k;
10833 cartesian_trajectory_planner_B.v_g[1] =
10834 cartesian_trajectory_planner_B.result_data_p[1] *
10835 cartesian_trajectory_planner_B.k;
10836 cartesian_trajectory_planner_B.v_g[2] =
10837 cartesian_trajectory_planner_B.result_data_p[2] *
10838 cartesian_trajectory_planner_B.k;
10839 cartesian_trajectory_planner_B.k = cos
10840 (cartesian_trajectory_planner_B.result_data_p[3]);
10841 cartesian_trajectory_planner_B.sth_p = sin
10842 (cartesian_trajectory_planner_B.result_data_p[3]);
10843 cartesian_trajectory_planner_B.tempR_e[0] =
10844 cartesian_trajectory_planner_B.v_g[0] *
10845 cartesian_trajectory_planner_B.v_g[0] * (1.0 -
10846 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
10847 cartesian_trajectory_planner_B.tempR_tmp_o =
10848 cartesian_trajectory_planner_B.v_g[1] *
10849 cartesian_trajectory_planner_B.v_g[0] * (1.0 -
10850 cartesian_trajectory_planner_B.k);
10851 cartesian_trajectory_planner_B.tempR_tmp_lm =
10852 cartesian_trajectory_planner_B.v_g[2] *
10853 cartesian_trajectory_planner_B.sth_p;
10854 cartesian_trajectory_planner_B.tempR_e[1] =
10855 cartesian_trajectory_planner_B.tempR_tmp_o -
10856 cartesian_trajectory_planner_B.tempR_tmp_lm;
10857 cartesian_trajectory_planner_B.tempR_tmp_k =
10858 cartesian_trajectory_planner_B.v_g[2] *
10859 cartesian_trajectory_planner_B.v_g[0] * (1.0 -
10860 cartesian_trajectory_planner_B.k);
10861 cartesian_trajectory_planner_B.tempR_tmp_j =
10862 cartesian_trajectory_planner_B.v_g[1] *
10863 cartesian_trajectory_planner_B.sth_p;
10864 cartesian_trajectory_planner_B.tempR_e[2] =
10865 cartesian_trajectory_planner_B.tempR_tmp_k +
10866 cartesian_trajectory_planner_B.tempR_tmp_j;
10867 cartesian_trajectory_planner_B.tempR_e[3] =
10868 cartesian_trajectory_planner_B.tempR_tmp_o +
10869 cartesian_trajectory_planner_B.tempR_tmp_lm;
10870 cartesian_trajectory_planner_B.tempR_e[4] =
10871 cartesian_trajectory_planner_B.v_g[1] *
10872 cartesian_trajectory_planner_B.v_g[1] * (1.0 -
10873 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
10874 cartesian_trajectory_planner_B.tempR_tmp_o =
10875 cartesian_trajectory_planner_B.v_g[2] *
10876 cartesian_trajectory_planner_B.v_g[1] * (1.0 -
10877 cartesian_trajectory_planner_B.k);
10878 cartesian_trajectory_planner_B.tempR_tmp_lm =
10879 cartesian_trajectory_planner_B.v_g[0] *
10880 cartesian_trajectory_planner_B.sth_p;
10881 cartesian_trajectory_planner_B.tempR_e[5] =
10882 cartesian_trajectory_planner_B.tempR_tmp_o -
10883 cartesian_trajectory_planner_B.tempR_tmp_lm;
10884 cartesian_trajectory_planner_B.tempR_e[6] =
10885 cartesian_trajectory_planner_B.tempR_tmp_k -
10886 cartesian_trajectory_planner_B.tempR_tmp_j;
10887 cartesian_trajectory_planner_B.tempR_e[7] =
10888 cartesian_trajectory_planner_B.tempR_tmp_o +
10889 cartesian_trajectory_planner_B.tempR_tmp_lm;
10890 cartesian_trajectory_planner_B.tempR_e[8] =
10891 cartesian_trajectory_planner_B.v_g[2] *
10892 cartesian_trajectory_planner_B.v_g[2] * (1.0 -
10893 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
10894 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10895 cartesian_trajectory_planner_B.b_kstr_d < 3;
10896 cartesian_trajectory_planner_B.b_kstr_d++) {
10897 cartesian_trajectory_planner_B.e_kb =
10898 cartesian_trajectory_planner_B.b_kstr_d + 1;
10899 cartesian_trajectory_planner_B.R_p[cartesian_trajectory_planner_B.e_kb -
10900 1] = cartesian_trajectory_planner_B.tempR_e
10901 [(cartesian_trajectory_planner_B.e_kb - 1) * 3];
10902 cartesian_trajectory_planner_B.e_kb =
10903 cartesian_trajectory_planner_B.b_kstr_d + 1;
10904 cartesian_trajectory_planner_B.R_p[cartesian_trajectory_planner_B.e_kb +
10905 2] = cartesian_trajectory_planner_B.tempR_e
10906 [(cartesian_trajectory_planner_B.e_kb - 1) * 3 + 1];
10907 cartesian_trajectory_planner_B.e_kb =
10908 cartesian_trajectory_planner_B.b_kstr_d + 1;
10909 cartesian_trajectory_planner_B.R_p[cartesian_trajectory_planner_B.e_kb +
10910 5] = cartesian_trajectory_planner_B.tempR_e
10911 [(cartesian_trajectory_planner_B.e_kb - 1) * 3 + 2];
10912 }
10913
10914 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10915 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10916 cartesian_trajectory_planner_B.b_kstr_d < 3;
10917 cartesian_trajectory_planner_B.b_kstr_d++) {
10918 cartesian_trajectory_planner_B.d_l =
10919 cartesian_trajectory_planner_B.b_kstr_d << 2;
10920 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l] =
10921 cartesian_trajectory_planner_B.R_p[3 *
10922 cartesian_trajectory_planner_B.b_kstr_d];
10923 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l +
10924 1] = cartesian_trajectory_planner_B.R_p[3 *
10925 cartesian_trajectory_planner_B.b_kstr_d + 1];
10926 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l +
10927 2] = cartesian_trajectory_planner_B.R_p[3 *
10928 cartesian_trajectory_planner_B.b_kstr_d + 2];
10929 }
10930
10931 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
10932 break;
10933
10934 default:
10935 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
10936 cartesian_trajectory_planner_B.v_g);
10937 memset(&cartesian_trajectory_planner_B.tempR_e[0], 0, 9U * sizeof(real_T));
10938 cartesian_trajectory_planner_B.tempR_e[0] = 1.0;
10939 cartesian_trajectory_planner_B.tempR_e[4] = 1.0;
10940 cartesian_trajectory_planner_B.tempR_e[8] = 1.0;
10941 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10942 cartesian_trajectory_planner_B.b_kstr_d < 3;
10943 cartesian_trajectory_planner_B.b_kstr_d++) {
10944 cartesian_trajectory_planner_B.d_l =
10945 cartesian_trajectory_planner_B.b_kstr_d << 2;
10946 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l] =
10947 cartesian_trajectory_planner_B.tempR_e[3 *
10948 cartesian_trajectory_planner_B.b_kstr_d];
10949 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l +
10950 1] = cartesian_trajectory_planner_B.tempR_e[3 *
10951 cartesian_trajectory_planner_B.b_kstr_d + 1];
10952 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l +
10953 2] = cartesian_trajectory_planner_B.tempR_e[3 *
10954 cartesian_trajectory_planner_B.b_kstr_d + 2];
10955 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_d
10956 + 12] =
10957 cartesian_trajectory_planner_B.v_g[cartesian_trajectory_planner_B.b_kstr_d]
10958 * qvec[cartesian_trajectory_planner_B.e_kb];
10959 }
10960
10961 cartesian_trajectory_planner_B.c_f1[3] = 0.0;
10962 cartesian_trajectory_planner_B.c_f1[7] = 0.0;
10963 cartesian_trajectory_planner_B.c_f1[11] = 0.0;
10964 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
10965 break;
10966 }
10967
10968 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10969 cartesian_trajectory_planner_B.b_kstr_d < 16;
10970 cartesian_trajectory_planner_B.b_kstr_d++) {
10971 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d]
10972 = body->
10973 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_d];
10974 }
10975
10976 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10977 cartesian_trajectory_planner_B.b_kstr_d < 16;
10978 cartesian_trajectory_planner_B.b_kstr_d++) {
10979 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.b_kstr_d]
10980 = body->
10981 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_d];
10982 }
10983
10984 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
10985 cartesian_trajectory_planner_B.b_kstr_d < 4;
10986 cartesian_trajectory_planner_B.b_kstr_d++) {
10987 for (cartesian_trajectory_planner_B.e_kb = 0;
10988 cartesian_trajectory_planner_B.e_kb < 4;
10989 cartesian_trajectory_planner_B.e_kb++) {
10990 cartesian_trajectory_planner_B.d_l = cartesian_trajectory_planner_B.e_kb
10991 << 2;
10992 cartesian_trajectory_planner_B.loop_ub_n =
10993 cartesian_trajectory_planner_B.b_kstr_d +
10994 cartesian_trajectory_planner_B.d_l;
10995 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
10996 = 0.0;
10997 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
10998 +=
10999 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l]
11000 * cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d];
11001 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11002 +=
11003 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l
11004 + 1] *
11005 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11006 + 4];
11007 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11008 +=
11009 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l
11010 + 2] *
11011 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11012 + 8];
11013 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11014 +=
11015 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_l
11016 + 3] *
11017 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11018 + 12];
11019 }
11020
11021 for (cartesian_trajectory_planner_B.e_kb = 0;
11022 cartesian_trajectory_planner_B.e_kb < 4;
11023 cartesian_trajectory_planner_B.e_kb++) {
11024 cartesian_trajectory_planner_B.d_l = cartesian_trajectory_planner_B.e_kb
11025 << 2;
11026 cartesian_trajectory_planner_B.loop_ub_n =
11027 cartesian_trajectory_planner_B.b_kstr_d +
11028 cartesian_trajectory_planner_B.d_l;
11029 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11030 .f1[cartesian_trajectory_planner_B.loop_ub_n] = 0.0;
11031 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11032 .f1[cartesian_trajectory_planner_B.loop_ub_n] +=
11033 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.d_l]
11034 * cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.b_kstr_d];
11035 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11036 .f1[cartesian_trajectory_planner_B.loop_ub_n] +=
11037 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.d_l
11038 + 1] *
11039 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.b_kstr_d
11040 + 4];
11041 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11042 .f1[cartesian_trajectory_planner_B.loop_ub_n] +=
11043 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.d_l
11044 + 2] *
11045 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.b_kstr_d
11046 + 8];
11047 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11048 .f1[cartesian_trajectory_planner_B.loop_ub_n] +=
11049 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.d_l
11050 + 3] *
11051 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.b_kstr_d
11052 + 12];
11053 }
11054 }
11055
11056 cartesian_trajectory_planner_B.k = cartesian_trajectory_planner_B.n;
11057 if (body->ParentIndex > 0.0) {
11058 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
11059 cartesian_trajectory_planner_B.b_kstr_d < 16;
11060 cartesian_trajectory_planner_B.b_kstr_d++) {
11061 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d]
11062 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
11063 .f1[cartesian_trajectory_planner_B.b_kstr_d];
11064 }
11065
11066 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
11067 cartesian_trajectory_planner_B.b_kstr_d < 4;
11068 cartesian_trajectory_planner_B.b_kstr_d++) {
11069 for (cartesian_trajectory_planner_B.e_kb = 0;
11070 cartesian_trajectory_planner_B.e_kb < 4;
11071 cartesian_trajectory_planner_B.e_kb++) {
11072 cartesian_trajectory_planner_B.d_l =
11073 cartesian_trajectory_planner_B.e_kb << 2;
11074 cartesian_trajectory_planner_B.loop_ub_n =
11075 cartesian_trajectory_planner_B.b_kstr_d +
11076 cartesian_trajectory_planner_B.d_l;
11077 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11078 = 0.0;
11079 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11080 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11081 .f1[cartesian_trajectory_planner_B.d_l] *
11082 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d];
11083 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11084 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11085 .f1[cartesian_trajectory_planner_B.d_l + 1] *
11086 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11087 + 4];
11088 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11089 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11090 .f1[cartesian_trajectory_planner_B.d_l + 2] *
11091 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11092 + 8];
11093 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.loop_ub_n]
11094 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11095 .f1[cartesian_trajectory_planner_B.d_l + 3] *
11096 cartesian_trajectory_planner_B.a_o[cartesian_trajectory_planner_B.b_kstr_d
11097 + 12];
11098 }
11099 }
11100
11101 for (cartesian_trajectory_planner_B.b_kstr_d = 0;
11102 cartesian_trajectory_planner_B.b_kstr_d < 16;
11103 cartesian_trajectory_planner_B.b_kstr_d++) {
11104 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11105 .f1[cartesian_trajectory_planner_B.b_kstr_d] =
11106 cartesian_trajectory_planner_B.a_n[cartesian_trajectory_planner_B.b_kstr_d];
11107 }
11108 }
11109 }
11110
11111 cartesian_trajec_emxFree_char_T(&switch_expression);
11112}
11113
11114static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
11115 **pEmxArray)
11116{
11117 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
11118 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_traject_T *)NULL) &&
11119 (*pEmxArray)->canFreeData) {
11120 free((*pEmxArray)->data);
11121 }
11122
11123 free((*pEmxArray)->size);
11124 free(*pEmxArray);
11125 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
11126 }
11127}
11128
11129static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_Rig_T *obj,
11130 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac)
11131{
11132 emxArray_f_cell_wrap_cartesia_T *Ttree;
11133 n_robotics_manip_internal_Rig_T *body;
11134 emxArray_real_T_cartesian_tra_T *JacSlice;
11135 emxArray_char_T_cartesian_tra_T *bname;
11136 emxArray_real_T_cartesian_tra_T *b;
11137 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
11138 'e', 'e' };
11139
11140 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
11141
11142 int32_T exitg1;
11143 boolean_T exitg2;
11144 cartesian_t_emxInit_f_cell_wrap(&Ttree, 2);
11145 RigidBodyTree_forwardKinematics(obj, Q, Ttree);
11146 cartesian_trajectory_planner_B.b_kstr_h = Jac->size[0] * Jac->size[1];
11147 Jac->size[0] = 6;
11148 Jac->size[1] = static_cast<int32_T>(obj->VelocityNumber);
11149 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_h);
11150 cartesian_trajectory_planner_B.loop_ub_g3 = 6 * static_cast<int32_T>
11151 (obj->VelocityNumber) - 1;
11152 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11153 cartesian_trajectory_planner_B.b_kstr_h <=
11154 cartesian_trajectory_planner_B.loop_ub_g3;
11155 cartesian_trajectory_planner_B.b_kstr_h++) {
11156 Jac->data[cartesian_trajectory_planner_B.b_kstr_h] = 0.0;
11157 }
11158
11159 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11160 cartesian_trajectory_planner_B.b_kstr_h < 8;
11161 cartesian_trajectory_planner_B.b_kstr_h++) {
11162 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_kstr_h]
11163 = 0;
11164 }
11165
11166 cartesian_trajec_emxInit_char_T(&bname, 2);
11167 cartesian_trajectory_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
11168 bname->size[0] = 1;
11169 bname->size[1] = obj->Base.NameInternal->size[1];
11170 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr_h);
11171 cartesian_trajectory_planner_B.loop_ub_g3 = obj->Base.NameInternal->size[0] *
11172 obj->Base.NameInternal->size[1] - 1;
11173 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11174 cartesian_trajectory_planner_B.b_kstr_h <=
11175 cartesian_trajectory_planner_B.loop_ub_g3;
11176 cartesian_trajectory_planner_B.b_kstr_h++) {
11177 bname->data[cartesian_trajectory_planner_B.b_kstr_h] =
11178 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h];
11179 }
11180
11181 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11182 cartesian_trajectory_planner_B.b_kstr_h < 11;
11183 cartesian_trajectory_planner_B.b_kstr_h++) {
11184 cartesian_trajectory_planner_B.a_p2[cartesian_trajectory_planner_B.b_kstr_h]
11185 = tmp[cartesian_trajectory_planner_B.b_kstr_h];
11186 }
11187
11188 cartesian_trajectory_planner_B.b_bool_b = false;
11189 if (11 == bname->size[1]) {
11190 cartesian_trajectory_planner_B.b_kstr_h = 1;
11191 do {
11192 exitg1 = 0;
11193 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 11) {
11194 cartesian_trajectory_planner_B.kstr =
11195 cartesian_trajectory_planner_B.b_kstr_h - 1;
11196 if (cartesian_trajectory_planner_B.a_p2[cartesian_trajectory_planner_B.kstr]
11197 != bname->data[cartesian_trajectory_planner_B.kstr]) {
11198 exitg1 = 1;
11199 } else {
11200 cartesian_trajectory_planner_B.b_kstr_h++;
11201 }
11202 } else {
11203 cartesian_trajectory_planner_B.b_bool_b = true;
11204 exitg1 = 1;
11205 }
11206 } while (exitg1 == 0);
11207 }
11208
11209 if (cartesian_trajectory_planner_B.b_bool_b) {
11210 memset(&cartesian_trajectory_planner_B.T2inv[0], 0, sizeof(real_T) << 4U);
11211 cartesian_trajectory_planner_B.T2inv[0] = 1.0;
11212 cartesian_trajectory_planner_B.T2inv[5] = 1.0;
11213 cartesian_trajectory_planner_B.T2inv[10] = 1.0;
11214 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
11215 memset(&cartesian_trajectory_planner_B.T2[0], 0, sizeof(real_T) << 4U);
11216 cartesian_trajectory_planner_B.T2[0] = 1.0;
11217 cartesian_trajectory_planner_B.T2[5] = 1.0;
11218 cartesian_trajectory_planner_B.T2[10] = 1.0;
11219 cartesian_trajectory_planner_B.T2[15] = 1.0;
11220 } else {
11221 cartesian_trajectory_planner_B.endeffectorIndex = -1.0;
11222 cartesian_trajectory_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
11223 bname->size[0] = 1;
11224 bname->size[1] = obj->Base.NameInternal->size[1];
11225 cartes_emxEnsureCapacity_char_T(bname,
11226 cartesian_trajectory_planner_B.b_kstr_h);
11227 cartesian_trajectory_planner_B.loop_ub_g3 = obj->Base.NameInternal->size[0] *
11228 obj->Base.NameInternal->size[1] - 1;
11229 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11230 cartesian_trajectory_planner_B.b_kstr_h <=
11231 cartesian_trajectory_planner_B.loop_ub_g3;
11232 cartesian_trajectory_planner_B.b_kstr_h++) {
11233 bname->data[cartesian_trajectory_planner_B.b_kstr_h] =
11234 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h];
11235 }
11236
11237 cartesian_trajectory_planner_B.b_bool_b = false;
11238 if (bname->size[1] == 11) {
11239 cartesian_trajectory_planner_B.b_kstr_h = 1;
11240 do {
11241 exitg1 = 0;
11242 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 11) {
11243 cartesian_trajectory_planner_B.kstr =
11244 cartesian_trajectory_planner_B.b_kstr_h - 1;
11245 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11246 cartesian_trajectory_planner_B.a_p2[cartesian_trajectory_planner_B.kstr])
11247 {
11248 exitg1 = 1;
11249 } else {
11250 cartesian_trajectory_planner_B.b_kstr_h++;
11251 }
11252 } else {
11253 cartesian_trajectory_planner_B.b_bool_b = true;
11254 exitg1 = 1;
11255 }
11256 } while (exitg1 == 0);
11257 }
11258
11259 if (cartesian_trajectory_planner_B.b_bool_b) {
11260 cartesian_trajectory_planner_B.endeffectorIndex = 0.0;
11261 } else {
11262 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
11263 cartesian_trajectory_planner_B.b_i_oc = 0;
11264 exitg2 = false;
11265 while ((!exitg2) && (cartesian_trajectory_planner_B.b_i_oc <= static_cast<
11266 int32_T>(cartesian_trajectory_planner_B.idx_idx_1) -
11267 1)) {
11268 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_oc];
11269 cartesian_trajectory_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
11270 bname->size[0] = 1;
11271 bname->size[1] = body->NameInternal->size[1];
11272 cartes_emxEnsureCapacity_char_T(bname,
11273 cartesian_trajectory_planner_B.b_kstr_h);
11274 cartesian_trajectory_planner_B.loop_ub_g3 = body->NameInternal->size[0] *
11275 body->NameInternal->size[1] - 1;
11276 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11277 cartesian_trajectory_planner_B.b_kstr_h <=
11278 cartesian_trajectory_planner_B.loop_ub_g3;
11279 cartesian_trajectory_planner_B.b_kstr_h++) {
11280 bname->data[cartesian_trajectory_planner_B.b_kstr_h] =
11281 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h];
11282 }
11283
11284 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11285 cartesian_trajectory_planner_B.b_kstr_h < 11;
11286 cartesian_trajectory_planner_B.b_kstr_h++) {
11287 cartesian_trajectory_planner_B.a_p2[cartesian_trajectory_planner_B.b_kstr_h]
11288 = tmp[cartesian_trajectory_planner_B.b_kstr_h];
11289 }
11290
11291 cartesian_trajectory_planner_B.b_bool_b = false;
11292 if (bname->size[1] == 11) {
11293 cartesian_trajectory_planner_B.b_kstr_h = 1;
11294 do {
11295 exitg1 = 0;
11296 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 11) {
11297 cartesian_trajectory_planner_B.kstr =
11298 cartesian_trajectory_planner_B.b_kstr_h - 1;
11299 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11300 cartesian_trajectory_planner_B.a_p2[cartesian_trajectory_planner_B.kstr])
11301 {
11302 exitg1 = 1;
11303 } else {
11304 cartesian_trajectory_planner_B.b_kstr_h++;
11305 }
11306 } else {
11307 cartesian_trajectory_planner_B.b_bool_b = true;
11308 exitg1 = 1;
11309 }
11310 } while (exitg1 == 0);
11311 }
11312
11313 if (cartesian_trajectory_planner_B.b_bool_b) {
11314 cartesian_trajectory_planner_B.endeffectorIndex = static_cast<real_T>
11315 (cartesian_trajectory_planner_B.b_i_oc) + 1.0;
11316 exitg2 = true;
11317 } else {
11318 cartesian_trajectory_planner_B.b_i_oc++;
11319 }
11320 }
11321 }
11322
11323 cartesian_trajectory_planner_B.b_i_oc = static_cast<int32_T>
11324 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
11325 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_oc];
11326 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11327 cartesian_trajectory_planner_B.b_kstr_h < 16;
11328 cartesian_trajectory_planner_B.b_kstr_h++) {
11329 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_kstr_h]
11330 = Ttree->data[cartesian_trajectory_planner_B.b_i_oc]
11331 .f1[cartesian_trajectory_planner_B.b_kstr_h];
11332 }
11333
11334 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11335 cartesian_trajectory_planner_B.b_kstr_h < 3;
11336 cartesian_trajectory_planner_B.b_kstr_h++) {
11337 cartesian_trajectory_planner_B.R_cn[3 *
11338 cartesian_trajectory_planner_B.b_kstr_h] = Ttree->
11339 data[cartesian_trajectory_planner_B.b_i_oc]
11340 .f1[cartesian_trajectory_planner_B.b_kstr_h];
11341 cartesian_trajectory_planner_B.R_cn[3 *
11342 cartesian_trajectory_planner_B.b_kstr_h + 1] = Ttree->
11343 data[cartesian_trajectory_planner_B.b_i_oc]
11344 .f1[cartesian_trajectory_planner_B.b_kstr_h + 4];
11345 cartesian_trajectory_planner_B.R_cn[3 *
11346 cartesian_trajectory_planner_B.b_kstr_h + 2] = Ttree->
11347 data[cartesian_trajectory_planner_B.b_i_oc]
11348 .f1[cartesian_trajectory_planner_B.b_kstr_h + 8];
11349 }
11350
11351 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11352 cartesian_trajectory_planner_B.b_kstr_h < 9;
11353 cartesian_trajectory_planner_B.b_kstr_h++) {
11354 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h]
11355 =
11356 -cartesian_trajectory_planner_B.R_cn[cartesian_trajectory_planner_B.b_kstr_h];
11357 }
11358
11359 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11360 cartesian_trajectory_planner_B.b_kstr_h < 3;
11361 cartesian_trajectory_planner_B.b_kstr_h++) {
11362 cartesian_trajectory_planner_B.endeffectorIndex = Ttree->
11363 data[cartesian_trajectory_planner_B.b_i_oc].f1[12] *
11364 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h];
11365 cartesian_trajectory_planner_B.loop_ub_g3 =
11366 cartesian_trajectory_planner_B.b_kstr_h << 2;
11367 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_g3]
11368 = cartesian_trajectory_planner_B.R_cn[3 *
11369 cartesian_trajectory_planner_B.b_kstr_h];
11370 cartesian_trajectory_planner_B.endeffectorIndex +=
11371 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h
11372 + 3] * Ttree->data[cartesian_trajectory_planner_B.b_i_oc].f1[13];
11373 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_g3
11374 + 1] = cartesian_trajectory_planner_B.R_cn[3 *
11375 cartesian_trajectory_planner_B.b_kstr_h + 1];
11376 cartesian_trajectory_planner_B.endeffectorIndex +=
11377 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h
11378 + 6] * Ttree->data[cartesian_trajectory_planner_B.b_i_oc].f1[14];
11379 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_g3
11380 + 2] = cartesian_trajectory_planner_B.R_cn[3 *
11381 cartesian_trajectory_planner_B.b_kstr_h + 2];
11382 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_h
11383 + 12] = cartesian_trajectory_planner_B.endeffectorIndex;
11384 }
11385
11386 cartesian_trajectory_planner_B.T2inv[3] = 0.0;
11387 cartesian_trajectory_planner_B.T2inv[7] = 0.0;
11388 cartesian_trajectory_planner_B.T2inv[11] = 0.0;
11389 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
11390 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_oc]
11391 = 1;
11392 while (body->ParentIndex > 0.0) {
11393 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
11394 cartesian_trajectory_planner_B.chainmask[static_cast<int32_T>(body->Index)
11395 - 1] = 1;
11396 }
11397 }
11398
11399 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
11400 cartesian_trajectory_planner_B.c_h = static_cast<int32_T>
11401 (cartesian_trajectory_planner_B.idx_idx_1) - 1;
11402 cartesian_trajec_emxInit_real_T(&JacSlice, 2);
11403 cartesian_trajec_emxInit_real_T(&b, 2);
11404 if (0 <= cartesian_trajectory_planner_B.c_h) {
11405 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11406 cartesian_trajectory_planner_B.b_kstr_h < 5;
11407 cartesian_trajectory_planner_B.b_kstr_h++) {
11408 cartesian_trajectory_planner_B.b_jw[cartesian_trajectory_planner_B.b_kstr_h]
11409 = tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
11410 }
11411 }
11412
11413 for (cartesian_trajectory_planner_B.b_i_oc = 0;
11414 cartesian_trajectory_planner_B.b_i_oc <=
11415 cartesian_trajectory_planner_B.c_h; cartesian_trajectory_planner_B.b_i_oc
11416 ++) {
11417 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_oc];
11418 cartesian_trajectory_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
11419 bname->size[0] = 1;
11420 bname->size[1] = body->JointInternal.Type->size[1];
11421 cartes_emxEnsureCapacity_char_T(bname,
11422 cartesian_trajectory_planner_B.b_kstr_h);
11423 cartesian_trajectory_planner_B.loop_ub_g3 = body->JointInternal.Type->size[0]
11424 * body->JointInternal.Type->size[1] - 1;
11425 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11426 cartesian_trajectory_planner_B.b_kstr_h <=
11427 cartesian_trajectory_planner_B.loop_ub_g3;
11428 cartesian_trajectory_planner_B.b_kstr_h++) {
11429 bname->data[cartesian_trajectory_planner_B.b_kstr_h] =
11430 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_h];
11431 }
11432
11433 cartesian_trajectory_planner_B.b_bool_b = false;
11434 if (bname->size[1] == 5) {
11435 cartesian_trajectory_planner_B.b_kstr_h = 1;
11436 do {
11437 exitg1 = 0;
11438 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 5) {
11439 cartesian_trajectory_planner_B.kstr =
11440 cartesian_trajectory_planner_B.b_kstr_h - 1;
11441 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11442 cartesian_trajectory_planner_B.b_jw[cartesian_trajectory_planner_B.kstr])
11443 {
11444 exitg1 = 1;
11445 } else {
11446 cartesian_trajectory_planner_B.b_kstr_h++;
11447 }
11448 } else {
11449 cartesian_trajectory_planner_B.b_bool_b = true;
11450 exitg1 = 1;
11451 }
11452 } while (exitg1 == 0);
11453 }
11454
11455 if ((!cartesian_trajectory_planner_B.b_bool_b) &&
11456 (cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_oc]
11457 != 0)) {
11458 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11459 cartesian_trajectory_planner_B.b_kstr_h < 16;
11460 cartesian_trajectory_planner_B.b_kstr_h++) {
11461 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.b_kstr_h]
11462 = Ttree->data[static_cast<int32_T>(body->Index) - 1]
11463 .f1[cartesian_trajectory_planner_B.b_kstr_h];
11464 }
11465
11466 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11467 cartesian_trajectory_planner_B.b_kstr_h < 16;
11468 cartesian_trajectory_planner_B.b_kstr_h++) {
11469 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h]
11470 = body->
11471 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_h];
11472 }
11473
11474 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11475 cartesian_trajectory_planner_B.b_kstr_h < 3;
11476 cartesian_trajectory_planner_B.b_kstr_h++) {
11477 cartesian_trajectory_planner_B.R_cn[3 *
11478 cartesian_trajectory_planner_B.b_kstr_h] =
11479 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h];
11480 cartesian_trajectory_planner_B.R_cn[3 *
11481 cartesian_trajectory_planner_B.b_kstr_h + 1] =
11482 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h
11483 + 4];
11484 cartesian_trajectory_planner_B.R_cn[3 *
11485 cartesian_trajectory_planner_B.b_kstr_h + 2] =
11486 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h
11487 + 8];
11488 }
11489
11490 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11491 cartesian_trajectory_planner_B.b_kstr_h < 9;
11492 cartesian_trajectory_planner_B.b_kstr_h++) {
11493 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h]
11494 =
11495 -cartesian_trajectory_planner_B.R_cn[cartesian_trajectory_planner_B.b_kstr_h];
11496 }
11497
11498 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11499 cartesian_trajectory_planner_B.b_kstr_h < 3;
11500 cartesian_trajectory_planner_B.b_kstr_h++) {
11501 cartesian_trajectory_planner_B.R_a5[cartesian_trajectory_planner_B.b_kstr_h]
11502 =
11503 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h
11504 + 6] * cartesian_trajectory_planner_B.Tdh[14] +
11505 (cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h
11506 + 3] * cartesian_trajectory_planner_B.Tdh[13] +
11507 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr_h]
11508 * cartesian_trajectory_planner_B.Tdh[12]);
11509 }
11510
11511 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11512 cartesian_trajectory_planner_B.b_kstr_h < 4;
11513 cartesian_trajectory_planner_B.b_kstr_h++) {
11514 for (cartesian_trajectory_planner_B.kstr = 0;
11515 cartesian_trajectory_planner_B.kstr < 4;
11516 cartesian_trajectory_planner_B.kstr++) {
11517 cartesian_trajectory_planner_B.n_j =
11518 cartesian_trajectory_planner_B.kstr << 2;
11519 cartesian_trajectory_planner_B.loop_ub_g3 =
11520 cartesian_trajectory_planner_B.b_kstr_h +
11521 cartesian_trajectory_planner_B.n_j;
11522 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_g3]
11523 = 0.0;
11524 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_g3]
11525 +=
11526 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.n_j]
11527 * cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_h];
11528 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_g3]
11529 +=
11530 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.n_j
11531 + 1] *
11532 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_h
11533 + 4];
11534 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_g3]
11535 +=
11536 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.n_j
11537 + 2] *
11538 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_h
11539 + 8];
11540 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_g3]
11541 +=
11542 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.n_j
11543 + 3] *
11544 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_h
11545 + 12];
11546 }
11547 }
11548
11549 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11550 cartesian_trajectory_planner_B.b_kstr_h < 3;
11551 cartesian_trajectory_planner_B.b_kstr_h++) {
11552 cartesian_trajectory_planner_B.kstr =
11553 cartesian_trajectory_planner_B.b_kstr_h << 2;
11554 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.kstr]
11555 = cartesian_trajectory_planner_B.R_cn[3 *
11556 cartesian_trajectory_planner_B.b_kstr_h];
11557 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.kstr
11558 + 1] = cartesian_trajectory_planner_B.R_cn[3 *
11559 cartesian_trajectory_planner_B.b_kstr_h + 1];
11560 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.kstr
11561 + 2] = cartesian_trajectory_planner_B.R_cn[3 *
11562 cartesian_trajectory_planner_B.b_kstr_h + 2];
11563 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.b_kstr_h
11564 + 12] =
11565 cartesian_trajectory_planner_B.R_a5[cartesian_trajectory_planner_B.b_kstr_h];
11566 }
11567
11568 cartesian_trajectory_planner_B.T1_l[3] = 0.0;
11569 cartesian_trajectory_planner_B.T1_l[7] = 0.0;
11570 cartesian_trajectory_planner_B.T1_l[11] = 0.0;
11571 cartesian_trajectory_planner_B.T1_l[15] = 1.0;
11572 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11573 cartesian_trajectory_planner_B.b_kstr_h < 4;
11574 cartesian_trajectory_planner_B.b_kstr_h++) {
11575 for (cartesian_trajectory_planner_B.kstr = 0;
11576 cartesian_trajectory_planner_B.kstr < 4;
11577 cartesian_trajectory_planner_B.kstr++) {
11578 cartesian_trajectory_planner_B.loop_ub_g3 =
11579 cartesian_trajectory_planner_B.kstr << 2;
11580 cartesian_trajectory_planner_B.n_j =
11581 cartesian_trajectory_planner_B.b_kstr_h +
11582 cartesian_trajectory_planner_B.loop_ub_g3;
11583 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] =
11584 0.0;
11585 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
11586 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.loop_ub_g3]
11587 * cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h];
11588 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
11589 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.loop_ub_g3
11590 + 1] *
11591 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h
11592 + 4];
11593 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
11594 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.loop_ub_g3
11595 + 2] *
11596 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h
11597 + 8];
11598 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
11599 cartesian_trajectory_planner_B.T1_l[cartesian_trajectory_planner_B.loop_ub_g3
11600 + 3] *
11601 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_h
11602 + 12];
11603 }
11604 }
11605
11606 cartesian_trajectory_planner_B.endeffectorIndex = obj->
11607 PositionDoFMap[cartesian_trajectory_planner_B.b_i_oc];
11608 cartesian_trajectory_planner_B.idx_idx_1 = obj->
11609 PositionDoFMap[cartesian_trajectory_planner_B.b_i_oc + 8];
11610 cartesian_trajectory_planner_B.R_cn[0] = 0.0;
11611 cartesian_trajectory_planner_B.R_cn[3] =
11612 -cartesian_trajectory_planner_B.T[14];
11613 cartesian_trajectory_planner_B.R_cn[6] = cartesian_trajectory_planner_B.T
11614 [13];
11615 cartesian_trajectory_planner_B.R_cn[1] = cartesian_trajectory_planner_B.T
11616 [14];
11617 cartesian_trajectory_planner_B.R_cn[4] = 0.0;
11618 cartesian_trajectory_planner_B.R_cn[7] =
11619 -cartesian_trajectory_planner_B.T[12];
11620 cartesian_trajectory_planner_B.R_cn[2] =
11621 -cartesian_trajectory_planner_B.T[13];
11622 cartesian_trajectory_planner_B.R_cn[5] = cartesian_trajectory_planner_B.T
11623 [12];
11624 cartesian_trajectory_planner_B.R_cn[8] = 0.0;
11625 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11626 cartesian_trajectory_planner_B.b_kstr_h < 3;
11627 cartesian_trajectory_planner_B.b_kstr_h++) {
11628 for (cartesian_trajectory_planner_B.kstr = 0;
11629 cartesian_trajectory_planner_B.kstr < 3;
11630 cartesian_trajectory_planner_B.kstr++) {
11631 cartesian_trajectory_planner_B.loop_ub_g3 =
11632 cartesian_trajectory_planner_B.b_kstr_h + 3 *
11633 cartesian_trajectory_planner_B.kstr;
11634 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_g3]
11635 = 0.0;
11636 cartesian_trajectory_planner_B.n_j =
11637 cartesian_trajectory_planner_B.kstr << 2;
11638 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_g3]
11639 +=
11640 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j]
11641 * cartesian_trajectory_planner_B.R_cn[cartesian_trajectory_planner_B.b_kstr_h];
11642 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_g3]
11643 +=
11644 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
11645 + 1] *
11646 cartesian_trajectory_planner_B.R_cn[cartesian_trajectory_planner_B.b_kstr_h
11647 + 3];
11648 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_g3]
11649 +=
11650 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
11651 + 2] *
11652 cartesian_trajectory_planner_B.R_cn[cartesian_trajectory_planner_B.b_kstr_h
11653 + 6];
11654 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
11655 + 6 * cartesian_trajectory_planner_B.b_kstr_h] =
11656 cartesian_trajectory_planner_B.T
11657 [(cartesian_trajectory_planner_B.b_kstr_h << 2) +
11658 cartesian_trajectory_planner_B.kstr];
11659 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
11660 + 6 * (cartesian_trajectory_planner_B.b_kstr_h + 3)] = 0.0;
11661 }
11662 }
11663
11664 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11665 cartesian_trajectory_planner_B.b_kstr_h < 3;
11666 cartesian_trajectory_planner_B.b_kstr_h++) {
11667 cartesian_trajectory_planner_B.X_f[6 *
11668 cartesian_trajectory_planner_B.b_kstr_h + 3] =
11669 cartesian_trajectory_planner_B.R_f[3 *
11670 cartesian_trajectory_planner_B.b_kstr_h];
11671 cartesian_trajectory_planner_B.kstr =
11672 cartesian_trajectory_planner_B.b_kstr_h << 2;
11673 cartesian_trajectory_planner_B.loop_ub_g3 = 6 *
11674 (cartesian_trajectory_planner_B.b_kstr_h + 3);
11675 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_g3
11676 + 3] =
11677 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr];
11678 cartesian_trajectory_planner_B.X_f[6 *
11679 cartesian_trajectory_planner_B.b_kstr_h + 4] =
11680 cartesian_trajectory_planner_B.R_f[3 *
11681 cartesian_trajectory_planner_B.b_kstr_h + 1];
11682 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_g3
11683 + 4] =
11684 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
11685 1];
11686 cartesian_trajectory_planner_B.X_f[6 *
11687 cartesian_trajectory_planner_B.b_kstr_h + 5] =
11688 cartesian_trajectory_planner_B.R_f[3 *
11689 cartesian_trajectory_planner_B.b_kstr_h + 2];
11690 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_g3
11691 + 5] =
11692 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
11693 2];
11694 }
11695
11696 cartesian_trajectory_planner_B.b_kstr_h = b->size[0] * b->size[1];
11697 b->size[0] = 6;
11698 b->size[1] = body->JointInternal.MotionSubspace->size[1];
11699 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr_h);
11700 cartesian_trajectory_planner_B.loop_ub_g3 =
11701 body->JointInternal.MotionSubspace->size[0] *
11702 body->JointInternal.MotionSubspace->size[1] - 1;
11703 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11704 cartesian_trajectory_planner_B.b_kstr_h <=
11705 cartesian_trajectory_planner_B.loop_ub_g3;
11706 cartesian_trajectory_planner_B.b_kstr_h++) {
11707 b->data[cartesian_trajectory_planner_B.b_kstr_h] =
11708 body->JointInternal.MotionSubspace->
11709 data[cartesian_trajectory_planner_B.b_kstr_h];
11710 }
11711
11712 cartesian_trajectory_planner_B.n_j = b->size[1] - 1;
11713 cartesian_trajectory_planner_B.b_kstr_h = JacSlice->size[0] *
11714 JacSlice->size[1];
11715 JacSlice->size[0] = 6;
11716 JacSlice->size[1] = b->size[1];
11717 cartes_emxEnsureCapacity_real_T(JacSlice,
11718 cartesian_trajectory_planner_B.b_kstr_h);
11719 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11720 cartesian_trajectory_planner_B.b_kstr_h <=
11721 cartesian_trajectory_planner_B.n_j;
11722 cartesian_trajectory_planner_B.b_kstr_h++) {
11723 cartesian_trajectory_planner_B.coffset_tmp_j =
11724 cartesian_trajectory_planner_B.b_kstr_h * 6 - 1;
11725 for (cartesian_trajectory_planner_B.kstr = 0;
11726 cartesian_trajectory_planner_B.kstr < 6;
11727 cartesian_trajectory_planner_B.kstr++) {
11728 cartesian_trajectory_planner_B.s_j = 0.0;
11729 for (cartesian_trajectory_planner_B.loop_ub_g3 = 0;
11730 cartesian_trajectory_planner_B.loop_ub_g3 < 6;
11731 cartesian_trajectory_planner_B.loop_ub_g3++) {
11732 cartesian_trajectory_planner_B.s_j +=
11733 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_g3
11734 * 6 + cartesian_trajectory_planner_B.kstr] * b->data
11735 [(cartesian_trajectory_planner_B.coffset_tmp_j +
11736 cartesian_trajectory_planner_B.loop_ub_g3) + 1];
11737 }
11738
11739 JacSlice->data[(cartesian_trajectory_planner_B.coffset_tmp_j +
11740 cartesian_trajectory_planner_B.kstr) + 1] =
11741 cartesian_trajectory_planner_B.s_j;
11742 }
11743 }
11744
11745 if (cartesian_trajectory_planner_B.endeffectorIndex >
11746 cartesian_trajectory_planner_B.idx_idx_1) {
11747 cartesian_trajectory_planner_B.n_j = 0;
11748 } else {
11749 cartesian_trajectory_planner_B.n_j = static_cast<int32_T>
11750 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
11751 }
11752
11753 cartesian_trajectory_planner_B.loop_ub_g3 = JacSlice->size[1];
11754 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11755 cartesian_trajectory_planner_B.b_kstr_h <
11756 cartesian_trajectory_planner_B.loop_ub_g3;
11757 cartesian_trajectory_planner_B.b_kstr_h++) {
11758 for (cartesian_trajectory_planner_B.kstr = 0;
11759 cartesian_trajectory_planner_B.kstr < 6;
11760 cartesian_trajectory_planner_B.kstr++) {
11761 Jac->data[cartesian_trajectory_planner_B.kstr + 6 *
11762 (cartesian_trajectory_planner_B.n_j +
11763 cartesian_trajectory_planner_B.b_kstr_h)] = JacSlice->data[6 *
11764 cartesian_trajectory_planner_B.b_kstr_h +
11765 cartesian_trajectory_planner_B.kstr];
11766 }
11767 }
11768 }
11769 }
11770
11771 cartesian_trajec_emxFree_char_T(&bname);
11772 cartesian_trajec_emxFree_real_T(&JacSlice);
11773 cartesian_t_emxFree_f_cell_wrap(&Ttree);
11774 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11775 cartesian_trajectory_planner_B.b_kstr_h < 3;
11776 cartesian_trajectory_planner_B.b_kstr_h++) {
11777 cartesian_trajectory_planner_B.b_i_oc =
11778 cartesian_trajectory_planner_B.b_kstr_h << 2;
11779 cartesian_trajectory_planner_B.X_f[6 *
11780 cartesian_trajectory_planner_B.b_kstr_h] =
11781 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_oc];
11782 cartesian_trajectory_planner_B.kstr = 6 *
11783 (cartesian_trajectory_planner_B.b_kstr_h + 3);
11784 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr] =
11785 0.0;
11786 cartesian_trajectory_planner_B.X_f[6 *
11787 cartesian_trajectory_planner_B.b_kstr_h + 3] = 0.0;
11788 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 3] =
11789 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_oc];
11790 cartesian_trajectory_planner_B.endeffectorIndex =
11791 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_oc +
11792 1];
11793 cartesian_trajectory_planner_B.X_f[6 *
11794 cartesian_trajectory_planner_B.b_kstr_h + 1] =
11795 cartesian_trajectory_planner_B.endeffectorIndex;
11796 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 1] =
11797 0.0;
11798 cartesian_trajectory_planner_B.X_f[6 *
11799 cartesian_trajectory_planner_B.b_kstr_h + 4] = 0.0;
11800 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 4] =
11801 cartesian_trajectory_planner_B.endeffectorIndex;
11802 cartesian_trajectory_planner_B.endeffectorIndex =
11803 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_oc +
11804 2];
11805 cartesian_trajectory_planner_B.X_f[6 *
11806 cartesian_trajectory_planner_B.b_kstr_h + 2] =
11807 cartesian_trajectory_planner_B.endeffectorIndex;
11808 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 2] =
11809 0.0;
11810 cartesian_trajectory_planner_B.X_f[6 *
11811 cartesian_trajectory_planner_B.b_kstr_h + 5] = 0.0;
11812 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 5] =
11813 cartesian_trajectory_planner_B.endeffectorIndex;
11814 }
11815
11816 cartesian_trajectory_planner_B.n_j = Jac->size[1];
11817 cartesian_trajectory_planner_B.b_kstr_h = b->size[0] * b->size[1];
11818 b->size[0] = 6;
11819 b->size[1] = Jac->size[1];
11820 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr_h);
11821 cartesian_trajectory_planner_B.loop_ub_g3 = Jac->size[0] * Jac->size[1] - 1;
11822 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11823 cartesian_trajectory_planner_B.b_kstr_h <=
11824 cartesian_trajectory_planner_B.loop_ub_g3;
11825 cartesian_trajectory_planner_B.b_kstr_h++) {
11826 b->data[cartesian_trajectory_planner_B.b_kstr_h] = Jac->
11827 data[cartesian_trajectory_planner_B.b_kstr_h];
11828 }
11829
11830 cartesian_trajectory_planner_B.b_kstr_h = Jac->size[0] * Jac->size[1];
11831 Jac->size[0] = 6;
11832 Jac->size[1] = cartesian_trajectory_planner_B.n_j;
11833 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_h);
11834 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
11835 cartesian_trajectory_planner_B.b_kstr_h <
11836 cartesian_trajectory_planner_B.n_j;
11837 cartesian_trajectory_planner_B.b_kstr_h++) {
11838 cartesian_trajectory_planner_B.coffset_tmp_j =
11839 cartesian_trajectory_planner_B.b_kstr_h * 6 - 1;
11840 for (cartesian_trajectory_planner_B.b_i_oc = 0;
11841 cartesian_trajectory_planner_B.b_i_oc < 6;
11842 cartesian_trajectory_planner_B.b_i_oc++) {
11843 cartesian_trajectory_planner_B.s_j = 0.0;
11844 for (cartesian_trajectory_planner_B.loop_ub_g3 = 0;
11845 cartesian_trajectory_planner_B.loop_ub_g3 < 6;
11846 cartesian_trajectory_planner_B.loop_ub_g3++) {
11847 cartesian_trajectory_planner_B.s_j +=
11848 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_g3
11849 * 6 + cartesian_trajectory_planner_B.b_i_oc] * b->data
11850 [(cartesian_trajectory_planner_B.coffset_tmp_j +
11851 cartesian_trajectory_planner_B.loop_ub_g3) + 1];
11852 }
11853
11854 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp_j +
11855 cartesian_trajectory_planner_B.b_i_oc) + 1] =
11856 cartesian_trajectory_planner_B.s_j;
11857 }
11858 }
11859
11860 cartesian_trajec_emxFree_real_T(&b);
11861}
11862
11863static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
11864 *obj, real_T ax[3])
11865{
11866 boolean_T b_bool;
11867 emxArray_char_T_cartesian_tra_T *a;
11868 int32_T loop_ub;
11869 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11870
11871 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
11872
11873 boolean_T guard1 = false;
11874 int32_T exitg1;
11875 cartesian_trajec_emxInit_char_T(&a, 2);
11876 cartesian_trajectory_planner_B.b_kstr_a = a->size[0] * a->size[1];
11877 a->size[0] = 1;
11878 a->size[1] = obj->Type->size[1];
11879 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_a);
11880 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
11881 for (cartesian_trajectory_planner_B.b_kstr_a = 0;
11882 cartesian_trajectory_planner_B.b_kstr_a <= loop_ub;
11883 cartesian_trajectory_planner_B.b_kstr_a++) {
11884 a->data[cartesian_trajectory_planner_B.b_kstr_a] = obj->Type->
11885 data[cartesian_trajectory_planner_B.b_kstr_a];
11886 }
11887
11888 for (cartesian_trajectory_planner_B.b_kstr_a = 0;
11889 cartesian_trajectory_planner_B.b_kstr_a < 8;
11890 cartesian_trajectory_planner_B.b_kstr_a++) {
11891 cartesian_trajectory_planner_B.b_i[cartesian_trajectory_planner_B.b_kstr_a] =
11892 tmp[cartesian_trajectory_planner_B.b_kstr_a];
11893 }
11894
11895 b_bool = false;
11896 if (a->size[1] == 8) {
11897 cartesian_trajectory_planner_B.b_kstr_a = 1;
11898 do {
11899 exitg1 = 0;
11900 if (cartesian_trajectory_planner_B.b_kstr_a - 1 < 8) {
11901 loop_ub = cartesian_trajectory_planner_B.b_kstr_a - 1;
11902 if (a->data[loop_ub] != cartesian_trajectory_planner_B.b_i[loop_ub]) {
11903 exitg1 = 1;
11904 } else {
11905 cartesian_trajectory_planner_B.b_kstr_a++;
11906 }
11907 } else {
11908 b_bool = true;
11909 exitg1 = 1;
11910 }
11911 } while (exitg1 == 0);
11912 }
11913
11914 guard1 = false;
11915 if (b_bool) {
11916 guard1 = true;
11917 } else {
11918 cartesian_trajectory_planner_B.b_kstr_a = a->size[0] * a->size[1];
11919 a->size[0] = 1;
11920 a->size[1] = obj->Type->size[1];
11921 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_a);
11922 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
11923 for (cartesian_trajectory_planner_B.b_kstr_a = 0;
11924 cartesian_trajectory_planner_B.b_kstr_a <= loop_ub;
11925 cartesian_trajectory_planner_B.b_kstr_a++) {
11926 a->data[cartesian_trajectory_planner_B.b_kstr_a] = obj->Type->
11927 data[cartesian_trajectory_planner_B.b_kstr_a];
11928 }
11929
11930 for (cartesian_trajectory_planner_B.b_kstr_a = 0;
11931 cartesian_trajectory_planner_B.b_kstr_a < 9;
11932 cartesian_trajectory_planner_B.b_kstr_a++) {
11933 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_a]
11934 = tmp_0[cartesian_trajectory_planner_B.b_kstr_a];
11935 }
11936
11937 b_bool = false;
11938 if (a->size[1] == 9) {
11939 cartesian_trajectory_planner_B.b_kstr_a = 1;
11940 do {
11941 exitg1 = 0;
11942 if (cartesian_trajectory_planner_B.b_kstr_a - 1 < 9) {
11943 loop_ub = cartesian_trajectory_planner_B.b_kstr_a - 1;
11944 if (a->data[loop_ub] != cartesian_trajectory_planner_B.b_g[loop_ub]) {
11945 exitg1 = 1;
11946 } else {
11947 cartesian_trajectory_planner_B.b_kstr_a++;
11948 }
11949 } else {
11950 b_bool = true;
11951 exitg1 = 1;
11952 }
11953 } while (exitg1 == 0);
11954 }
11955
11956 if (b_bool) {
11957 guard1 = true;
11958 } else {
11959 ax[0] = (rtNaN);
11960 ax[1] = (rtNaN);
11961 ax[2] = (rtNaN);
11962 }
11963 }
11964
11965 if (guard1) {
11966 ax[0] = obj->JointAxisInternal[0];
11967 ax[1] = obj->JointAxisInternal[1];
11968 ax[2] = obj->JointAxisInternal[2];
11969 }
11970
11971 cartesian_trajec_emxFree_char_T(&a);
11972}
11973
11974static void RigidBodyTree_forwardKinemati_a(p_robotics_manip_internal_R_a_T *obj,
11975 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
11976{
11977 n_robotics_manip_internal_R_a_T *body;
11978 emxArray_char_T_cartesian_tra_T *switch_expression;
11979 boolean_T b_bool;
11980 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
11981 };
11982
11983 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
11984
11985 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11986
11987 int32_T exitg1;
11988 cartesian_trajectory_planner_B.n_o = obj->NumBodies;
11989 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
11990 cartesian_trajectory_planner_B.b_kstr_e < 16;
11991 cartesian_trajectory_planner_B.b_kstr_e++) {
11992 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.b_kstr_e]
11993 = tmp[cartesian_trajectory_planner_B.b_kstr_e];
11994 }
11995
11996 cartesian_trajectory_planner_B.b_kstr_e = Ttree->size[0] * Ttree->size[1];
11997 Ttree->size[0] = 1;
11998 cartesian_trajectory_planner_B.e_n = static_cast<int32_T>
11999 (cartesian_trajectory_planner_B.n_o);
12000 Ttree->size[1] = cartesian_trajectory_planner_B.e_n;
12001 c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_trajectory_planner_B.b_kstr_e);
12002 if (cartesian_trajectory_planner_B.e_n != 0) {
12003 cartesian_trajectory_planner_B.ntilecols_f =
12004 cartesian_trajectory_planner_B.e_n - 1;
12005 if (0 <= cartesian_trajectory_planner_B.ntilecols_f) {
12006 memcpy(&cartesian_trajectory_planner_B.expl_temp_l.f1[0],
12007 &cartesian_trajectory_planner_B.c_f1_h[0], sizeof(real_T) << 4U);
12008 }
12009
12010 for (cartesian_trajectory_planner_B.b_jtilecol_n = 0;
12011 cartesian_trajectory_planner_B.b_jtilecol_n <=
12012 cartesian_trajectory_planner_B.ntilecols_f;
12013 cartesian_trajectory_planner_B.b_jtilecol_n++) {
12014 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n] =
12015 cartesian_trajectory_planner_B.expl_temp_l;
12016 }
12017 }
12018
12019 cartesian_trajectory_planner_B.k_f = 1.0;
12020 cartesian_trajectory_planner_B.ntilecols_f = static_cast<int32_T>
12021 (cartesian_trajectory_planner_B.n_o) - 1;
12022 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
12023 if (0 <= cartesian_trajectory_planner_B.ntilecols_f) {
12024 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12025 cartesian_trajectory_planner_B.b_kstr_e < 5;
12026 cartesian_trajectory_planner_B.b_kstr_e++) {
12027 cartesian_trajectory_planner_B.b_ew[cartesian_trajectory_planner_B.b_kstr_e]
12028 = tmp_0[cartesian_trajectory_planner_B.b_kstr_e];
12029 }
12030 }
12031
12032 for (cartesian_trajectory_planner_B.b_jtilecol_n = 0;
12033 cartesian_trajectory_planner_B.b_jtilecol_n <=
12034 cartesian_trajectory_planner_B.ntilecols_f;
12035 cartesian_trajectory_planner_B.b_jtilecol_n++) {
12036 body = obj->Bodies[cartesian_trajectory_planner_B.b_jtilecol_n];
12037 cartesian_trajectory_planner_B.n_o = body->JointInternal.PositionNumber;
12038 cartesian_trajectory_planner_B.n_o += cartesian_trajectory_planner_B.k_f;
12039 if (cartesian_trajectory_planner_B.k_f > cartesian_trajectory_planner_B.n_o
12040 - 1.0) {
12041 cartesian_trajectory_planner_B.e_n = 0;
12042 cartesian_trajectory_planner_B.d_e = 0;
12043 } else {
12044 cartesian_trajectory_planner_B.e_n = static_cast<int32_T>
12045 (cartesian_trajectory_planner_B.k_f) - 1;
12046 cartesian_trajectory_planner_B.d_e = static_cast<int32_T>
12047 (cartesian_trajectory_planner_B.n_o - 1.0);
12048 }
12049
12050 cartesian_trajectory_planner_B.b_kstr_e = switch_expression->size[0] *
12051 switch_expression->size[1];
12052 switch_expression->size[0] = 1;
12053 switch_expression->size[1] = body->JointInternal.Type->size[1];
12054 cartes_emxEnsureCapacity_char_T(switch_expression,
12055 cartesian_trajectory_planner_B.b_kstr_e);
12056 cartesian_trajectory_planner_B.loop_ub_by = body->JointInternal.Type->size[0]
12057 * body->JointInternal.Type->size[1] - 1;
12058 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12059 cartesian_trajectory_planner_B.b_kstr_e <=
12060 cartesian_trajectory_planner_B.loop_ub_by;
12061 cartesian_trajectory_planner_B.b_kstr_e++) {
12062 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_e] =
12063 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_e];
12064 }
12065
12066 b_bool = false;
12067 if (switch_expression->size[1] == 5) {
12068 cartesian_trajectory_planner_B.b_kstr_e = 1;
12069 do {
12070 exitg1 = 0;
12071 if (cartesian_trajectory_planner_B.b_kstr_e - 1 < 5) {
12072 cartesian_trajectory_planner_B.loop_ub_by =
12073 cartesian_trajectory_planner_B.b_kstr_e - 1;
12074 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_by]
12075 !=
12076 cartesian_trajectory_planner_B.b_ew[cartesian_trajectory_planner_B.loop_ub_by])
12077 {
12078 exitg1 = 1;
12079 } else {
12080 cartesian_trajectory_planner_B.b_kstr_e++;
12081 }
12082 } else {
12083 b_bool = true;
12084 exitg1 = 1;
12085 }
12086 } while (exitg1 == 0);
12087 }
12088
12089 if (b_bool) {
12090 cartesian_trajectory_planner_B.b_kstr_e = 0;
12091 } else {
12092 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12093 cartesian_trajectory_planner_B.b_kstr_e < 8;
12094 cartesian_trajectory_planner_B.b_kstr_e++) {
12095 cartesian_trajectory_planner_B.b_cx[cartesian_trajectory_planner_B.b_kstr_e]
12096 = tmp_1[cartesian_trajectory_planner_B.b_kstr_e];
12097 }
12098
12099 b_bool = false;
12100 if (switch_expression->size[1] == 8) {
12101 cartesian_trajectory_planner_B.b_kstr_e = 1;
12102 do {
12103 exitg1 = 0;
12104 if (cartesian_trajectory_planner_B.b_kstr_e - 1 < 8) {
12105 cartesian_trajectory_planner_B.loop_ub_by =
12106 cartesian_trajectory_planner_B.b_kstr_e - 1;
12107 if (switch_expression->
12108 data[cartesian_trajectory_planner_B.loop_ub_by] !=
12109 cartesian_trajectory_planner_B.b_cx[cartesian_trajectory_planner_B.loop_ub_by])
12110 {
12111 exitg1 = 1;
12112 } else {
12113 cartesian_trajectory_planner_B.b_kstr_e++;
12114 }
12115 } else {
12116 b_bool = true;
12117 exitg1 = 1;
12118 }
12119 } while (exitg1 == 0);
12120 }
12121
12122 if (b_bool) {
12123 cartesian_trajectory_planner_B.b_kstr_e = 1;
12124 } else {
12125 cartesian_trajectory_planner_B.b_kstr_e = -1;
12126 }
12127 }
12128
12129 switch (cartesian_trajectory_planner_B.b_kstr_e) {
12130 case 0:
12131 memset(&cartesian_trajectory_planner_B.c_f1_h[0], 0, sizeof(real_T) << 4U);
12132 cartesian_trajectory_planner_B.c_f1_h[0] = 1.0;
12133 cartesian_trajectory_planner_B.c_f1_h[5] = 1.0;
12134 cartesian_trajectory_planner_B.c_f1_h[10] = 1.0;
12135 cartesian_trajectory_planner_B.c_f1_h[15] = 1.0;
12136 break;
12137
12138 case 1:
12139 rigidBodyJoint_get_JointAxis_a(&body->JointInternal,
12140 cartesian_trajectory_planner_B.v_na);
12141 cartesian_trajectory_planner_B.d_e -= cartesian_trajectory_planner_B.e_n;
12142 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12143 cartesian_trajectory_planner_B.b_kstr_e <
12144 cartesian_trajectory_planner_B.d_e;
12145 cartesian_trajectory_planner_B.b_kstr_e++) {
12146 cartesian_trajectory_planner_B.e_data_c[cartesian_trajectory_planner_B.b_kstr_e]
12147 = cartesian_trajectory_planner_B.e_n +
12148 cartesian_trajectory_planner_B.b_kstr_e;
12149 }
12150
12151 cartesian_trajectory_planner_B.result_data_c[0] =
12152 cartesian_trajectory_planner_B.v_na[0];
12153 cartesian_trajectory_planner_B.result_data_c[1] =
12154 cartesian_trajectory_planner_B.v_na[1];
12155 cartesian_trajectory_planner_B.result_data_c[2] =
12156 cartesian_trajectory_planner_B.v_na[2];
12157 if (0 <= (cartesian_trajectory_planner_B.d_e != 0) - 1) {
12158 cartesian_trajectory_planner_B.result_data_c[3] =
12159 qvec[cartesian_trajectory_planner_B.e_data_c[0]];
12160 }
12161
12162 cartesian_trajectory_planner_B.k_f = 1.0 / sqrt
12163 ((cartesian_trajectory_planner_B.result_data_c[0] *
12164 cartesian_trajectory_planner_B.result_data_c[0] +
12165 cartesian_trajectory_planner_B.result_data_c[1] *
12166 cartesian_trajectory_planner_B.result_data_c[1]) +
12167 cartesian_trajectory_planner_B.result_data_c[2] *
12168 cartesian_trajectory_planner_B.result_data_c[2]);
12169 cartesian_trajectory_planner_B.v_na[0] =
12170 cartesian_trajectory_planner_B.result_data_c[0] *
12171 cartesian_trajectory_planner_B.k_f;
12172 cartesian_trajectory_planner_B.v_na[1] =
12173 cartesian_trajectory_planner_B.result_data_c[1] *
12174 cartesian_trajectory_planner_B.k_f;
12175 cartesian_trajectory_planner_B.v_na[2] =
12176 cartesian_trajectory_planner_B.result_data_c[2] *
12177 cartesian_trajectory_planner_B.k_f;
12178 cartesian_trajectory_planner_B.k_f = cos
12179 (cartesian_trajectory_planner_B.result_data_c[3]);
12180 cartesian_trajectory_planner_B.sth_o = sin
12181 (cartesian_trajectory_planner_B.result_data_c[3]);
12182 cartesian_trajectory_planner_B.tempR_o[0] =
12183 cartesian_trajectory_planner_B.v_na[0] *
12184 cartesian_trajectory_planner_B.v_na[0] * (1.0 -
12185 cartesian_trajectory_planner_B.k_f) + cartesian_trajectory_planner_B.k_f;
12186 cartesian_trajectory_planner_B.tempR_tmp_ln =
12187 cartesian_trajectory_planner_B.v_na[1] *
12188 cartesian_trajectory_planner_B.v_na[0] * (1.0 -
12189 cartesian_trajectory_planner_B.k_f);
12190 cartesian_trajectory_planner_B.tempR_tmp_lu =
12191 cartesian_trajectory_planner_B.v_na[2] *
12192 cartesian_trajectory_planner_B.sth_o;
12193 cartesian_trajectory_planner_B.tempR_o[1] =
12194 cartesian_trajectory_planner_B.tempR_tmp_ln -
12195 cartesian_trajectory_planner_B.tempR_tmp_lu;
12196 cartesian_trajectory_planner_B.tempR_tmp_g =
12197 cartesian_trajectory_planner_B.v_na[2] *
12198 cartesian_trajectory_planner_B.v_na[0] * (1.0 -
12199 cartesian_trajectory_planner_B.k_f);
12200 cartesian_trajectory_planner_B.tempR_tmp_d =
12201 cartesian_trajectory_planner_B.v_na[1] *
12202 cartesian_trajectory_planner_B.sth_o;
12203 cartesian_trajectory_planner_B.tempR_o[2] =
12204 cartesian_trajectory_planner_B.tempR_tmp_g +
12205 cartesian_trajectory_planner_B.tempR_tmp_d;
12206 cartesian_trajectory_planner_B.tempR_o[3] =
12207 cartesian_trajectory_planner_B.tempR_tmp_ln +
12208 cartesian_trajectory_planner_B.tempR_tmp_lu;
12209 cartesian_trajectory_planner_B.tempR_o[4] =
12210 cartesian_trajectory_planner_B.v_na[1] *
12211 cartesian_trajectory_planner_B.v_na[1] * (1.0 -
12212 cartesian_trajectory_planner_B.k_f) + cartesian_trajectory_planner_B.k_f;
12213 cartesian_trajectory_planner_B.tempR_tmp_ln =
12214 cartesian_trajectory_planner_B.v_na[2] *
12215 cartesian_trajectory_planner_B.v_na[1] * (1.0 -
12216 cartesian_trajectory_planner_B.k_f);
12217 cartesian_trajectory_planner_B.tempR_tmp_lu =
12218 cartesian_trajectory_planner_B.v_na[0] *
12219 cartesian_trajectory_planner_B.sth_o;
12220 cartesian_trajectory_planner_B.tempR_o[5] =
12221 cartesian_trajectory_planner_B.tempR_tmp_ln -
12222 cartesian_trajectory_planner_B.tempR_tmp_lu;
12223 cartesian_trajectory_planner_B.tempR_o[6] =
12224 cartesian_trajectory_planner_B.tempR_tmp_g -
12225 cartesian_trajectory_planner_B.tempR_tmp_d;
12226 cartesian_trajectory_planner_B.tempR_o[7] =
12227 cartesian_trajectory_planner_B.tempR_tmp_ln +
12228 cartesian_trajectory_planner_B.tempR_tmp_lu;
12229 cartesian_trajectory_planner_B.tempR_o[8] =
12230 cartesian_trajectory_planner_B.v_na[2] *
12231 cartesian_trajectory_planner_B.v_na[2] * (1.0 -
12232 cartesian_trajectory_planner_B.k_f) + cartesian_trajectory_planner_B.k_f;
12233 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12234 cartesian_trajectory_planner_B.b_kstr_e < 3;
12235 cartesian_trajectory_planner_B.b_kstr_e++) {
12236 cartesian_trajectory_planner_B.e_n =
12237 cartesian_trajectory_planner_B.b_kstr_e + 1;
12238 cartesian_trajectory_planner_B.R_e[cartesian_trajectory_planner_B.e_n -
12239 1] = cartesian_trajectory_planner_B.tempR_o
12240 [(cartesian_trajectory_planner_B.e_n - 1) * 3];
12241 cartesian_trajectory_planner_B.e_n =
12242 cartesian_trajectory_planner_B.b_kstr_e + 1;
12243 cartesian_trajectory_planner_B.R_e[cartesian_trajectory_planner_B.e_n +
12244 2] = cartesian_trajectory_planner_B.tempR_o
12245 [(cartesian_trajectory_planner_B.e_n - 1) * 3 + 1];
12246 cartesian_trajectory_planner_B.e_n =
12247 cartesian_trajectory_planner_B.b_kstr_e + 1;
12248 cartesian_trajectory_planner_B.R_e[cartesian_trajectory_planner_B.e_n +
12249 5] = cartesian_trajectory_planner_B.tempR_o
12250 [(cartesian_trajectory_planner_B.e_n - 1) * 3 + 2];
12251 }
12252
12253 memset(&cartesian_trajectory_planner_B.c_f1_h[0], 0, sizeof(real_T) << 4U);
12254 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12255 cartesian_trajectory_planner_B.b_kstr_e < 3;
12256 cartesian_trajectory_planner_B.b_kstr_e++) {
12257 cartesian_trajectory_planner_B.d_e =
12258 cartesian_trajectory_planner_B.b_kstr_e << 2;
12259 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e]
12260 = cartesian_trajectory_planner_B.R_e[3 *
12261 cartesian_trajectory_planner_B.b_kstr_e];
12262 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12263 + 1] = cartesian_trajectory_planner_B.R_e[3 *
12264 cartesian_trajectory_planner_B.b_kstr_e + 1];
12265 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12266 + 2] = cartesian_trajectory_planner_B.R_e[3 *
12267 cartesian_trajectory_planner_B.b_kstr_e + 2];
12268 }
12269
12270 cartesian_trajectory_planner_B.c_f1_h[15] = 1.0;
12271 break;
12272
12273 default:
12274 rigidBodyJoint_get_JointAxis_a(&body->JointInternal,
12275 cartesian_trajectory_planner_B.v_na);
12276 memset(&cartesian_trajectory_planner_B.tempR_o[0], 0, 9U * sizeof(real_T));
12277 cartesian_trajectory_planner_B.tempR_o[0] = 1.0;
12278 cartesian_trajectory_planner_B.tempR_o[4] = 1.0;
12279 cartesian_trajectory_planner_B.tempR_o[8] = 1.0;
12280 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12281 cartesian_trajectory_planner_B.b_kstr_e < 3;
12282 cartesian_trajectory_planner_B.b_kstr_e++) {
12283 cartesian_trajectory_planner_B.d_e =
12284 cartesian_trajectory_planner_B.b_kstr_e << 2;
12285 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e]
12286 = cartesian_trajectory_planner_B.tempR_o[3 *
12287 cartesian_trajectory_planner_B.b_kstr_e];
12288 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12289 + 1] = cartesian_trajectory_planner_B.tempR_o[3 *
12290 cartesian_trajectory_planner_B.b_kstr_e + 1];
12291 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12292 + 2] = cartesian_trajectory_planner_B.tempR_o[3 *
12293 cartesian_trajectory_planner_B.b_kstr_e + 2];
12294 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.b_kstr_e
12295 + 12] =
12296 cartesian_trajectory_planner_B.v_na[cartesian_trajectory_planner_B.b_kstr_e]
12297 * qvec[cartesian_trajectory_planner_B.e_n];
12298 }
12299
12300 cartesian_trajectory_planner_B.c_f1_h[3] = 0.0;
12301 cartesian_trajectory_planner_B.c_f1_h[7] = 0.0;
12302 cartesian_trajectory_planner_B.c_f1_h[11] = 0.0;
12303 cartesian_trajectory_planner_B.c_f1_h[15] = 1.0;
12304 break;
12305 }
12306
12307 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12308 cartesian_trajectory_planner_B.b_kstr_e < 16;
12309 cartesian_trajectory_planner_B.b_kstr_e++) {
12310 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e]
12311 = body->
12312 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_e];
12313 }
12314
12315 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12316 cartesian_trajectory_planner_B.b_kstr_e < 16;
12317 cartesian_trajectory_planner_B.b_kstr_e++) {
12318 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.b_kstr_e]
12319 = body->
12320 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_e];
12321 }
12322
12323 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12324 cartesian_trajectory_planner_B.b_kstr_e < 4;
12325 cartesian_trajectory_planner_B.b_kstr_e++) {
12326 for (cartesian_trajectory_planner_B.e_n = 0;
12327 cartesian_trajectory_planner_B.e_n < 4;
12328 cartesian_trajectory_planner_B.e_n++) {
12329 cartesian_trajectory_planner_B.d_e = cartesian_trajectory_planner_B.e_n <<
12330 2;
12331 cartesian_trajectory_planner_B.loop_ub_by =
12332 cartesian_trajectory_planner_B.b_kstr_e +
12333 cartesian_trajectory_planner_B.d_e;
12334 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12335 = 0.0;
12336 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12337 +=
12338 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e]
12339 * cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e];
12340 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12341 +=
12342 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12343 + 1] *
12344 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12345 + 4];
12346 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12347 +=
12348 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12349 + 2] *
12350 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12351 + 8];
12352 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12353 +=
12354 cartesian_trajectory_planner_B.c_f1_h[cartesian_trajectory_planner_B.d_e
12355 + 3] *
12356 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12357 + 12];
12358 }
12359
12360 for (cartesian_trajectory_planner_B.e_n = 0;
12361 cartesian_trajectory_planner_B.e_n < 4;
12362 cartesian_trajectory_planner_B.e_n++) {
12363 cartesian_trajectory_planner_B.d_e = cartesian_trajectory_planner_B.e_n <<
12364 2;
12365 cartesian_trajectory_planner_B.loop_ub_by =
12366 cartesian_trajectory_planner_B.b_kstr_e +
12367 cartesian_trajectory_planner_B.d_e;
12368 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12369 .f1[cartesian_trajectory_planner_B.loop_ub_by] = 0.0;
12370 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12371 .f1[cartesian_trajectory_planner_B.loop_ub_by] +=
12372 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.d_e]
12373 * cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_kstr_e];
12374 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12375 .f1[cartesian_trajectory_planner_B.loop_ub_by] +=
12376 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.d_e
12377 + 1] *
12378 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_kstr_e
12379 + 4];
12380 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12381 .f1[cartesian_trajectory_planner_B.loop_ub_by] +=
12382 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.d_e
12383 + 2] *
12384 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_kstr_e
12385 + 8];
12386 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12387 .f1[cartesian_trajectory_planner_B.loop_ub_by] +=
12388 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.d_e
12389 + 3] *
12390 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_kstr_e
12391 + 12];
12392 }
12393 }
12394
12395 cartesian_trajectory_planner_B.k_f = cartesian_trajectory_planner_B.n_o;
12396 if (body->ParentIndex > 0.0) {
12397 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12398 cartesian_trajectory_planner_B.b_kstr_e < 16;
12399 cartesian_trajectory_planner_B.b_kstr_e++) {
12400 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e]
12401 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
12402 .f1[cartesian_trajectory_planner_B.b_kstr_e];
12403 }
12404
12405 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12406 cartesian_trajectory_planner_B.b_kstr_e < 4;
12407 cartesian_trajectory_planner_B.b_kstr_e++) {
12408 for (cartesian_trajectory_planner_B.e_n = 0;
12409 cartesian_trajectory_planner_B.e_n < 4;
12410 cartesian_trajectory_planner_B.e_n++) {
12411 cartesian_trajectory_planner_B.d_e =
12412 cartesian_trajectory_planner_B.e_n << 2;
12413 cartesian_trajectory_planner_B.loop_ub_by =
12414 cartesian_trajectory_planner_B.b_kstr_e +
12415 cartesian_trajectory_planner_B.d_e;
12416 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12417 = 0.0;
12418 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12419 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12420 .f1[cartesian_trajectory_planner_B.d_e] *
12421 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e];
12422 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12423 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12424 .f1[cartesian_trajectory_planner_B.d_e + 1] *
12425 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12426 + 4];
12427 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12428 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12429 .f1[cartesian_trajectory_planner_B.d_e + 2] *
12430 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12431 + 8];
12432 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.loop_ub_by]
12433 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12434 .f1[cartesian_trajectory_planner_B.d_e + 3] *
12435 cartesian_trajectory_planner_B.a_c[cartesian_trajectory_planner_B.b_kstr_e
12436 + 12];
12437 }
12438 }
12439
12440 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
12441 cartesian_trajectory_planner_B.b_kstr_e < 16;
12442 cartesian_trajectory_planner_B.b_kstr_e++) {
12443 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol_n]
12444 .f1[cartesian_trajectory_planner_B.b_kstr_e] =
12445 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_kstr_e];
12446 }
12447 }
12448 }
12449
12450 cartesian_trajec_emxFree_char_T(&switch_expression);
12451}
12452
12453void rt_invd6x6_snf(const real_T u[36], real_T y[36])
12454{
12455 int8_T p[6];
12456 real_T A[36];
12457 int8_T ipiv[6];
12458 int32_T jj;
12459 int32_T j;
12460 int32_T kAcol;
12461 int32_T c;
12462 int32_T ix;
12463 real_T smax;
12464 real_T s;
12465 int32_T iy;
12466 int32_T jy;
12467 int32_T j_0;
12468 int32_T ijA;
12469 for (iy = 0; iy < 36; iy++) {
12470 y[iy] = 0.0;
12471 A[iy] = u[iy];
12472 }
12473
12474 for (iy = 0; iy < 6; iy++) {
12475 ipiv[iy] = static_cast<int8_T>(iy + 1);
12476 }
12477
12478 for (j = 0; j < 5; j++) {
12479 c = j * 7 + 2;
12480 jj = j * 7;
12481 kAcol = 6 - j;
12482 iy = 1;
12483 ix = c - 2;
12484 smax = fabs(A[jj]);
12485 for (jy = 2; jy <= kAcol; jy++) {
12486 ix++;
12487 s = fabs(A[ix]);
12488 if (s > smax) {
12489 iy = jy;
12490 smax = s;
12491 }
12492 }
12493
12494 if (A[(c + iy) - 3] != 0.0) {
12495 if (iy - 1 != 0) {
12496 jy = j + iy;
12497 ipiv[j] = static_cast<int8_T>(jy);
12498 ix = j;
12499 iy = jy - 1;
12500 for (jy = 0; jy < 6; jy++) {
12501 smax = A[ix];
12502 A[ix] = A[iy];
12503 A[iy] = smax;
12504 ix += 6;
12505 iy += 6;
12506 }
12507 }
12508
12509 iy = c - j;
12510 for (ix = c; ix <= iy + 4; ix++) {
12511 A[ix - 1] /= A[jj];
12512 }
12513 }
12514
12515 kAcol = 4 - j;
12516 jy = jj + 6;
12517 for (j_0 = 0; j_0 <= kAcol; j_0++) {
12518 if (A[jy] != 0.0) {
12519 smax = -A[jy];
12520 ix = c - 1;
12521 iy = jj - j;
12522 for (ijA = jj + 8; ijA <= iy + 12; ijA++) {
12523 A[ijA - 1] += A[ix] * smax;
12524 ix++;
12525 }
12526 }
12527
12528 jy += 6;
12529 jj += 6;
12530 }
12531 }
12532
12533 for (iy = 0; iy < 6; iy++) {
12534 p[iy] = static_cast<int8_T>(iy + 1);
12535 }
12536
12537 for (jy = 0; jy < 5; jy++) {
12538 if (ipiv[jy] > jy + 1) {
12539 j = ipiv[jy] - 1;
12540 iy = p[j];
12541 p[j] = p[jy];
12542 p[jy] = static_cast<int8_T>(iy);
12543 }
12544 }
12545
12546 for (jy = 0; jy < 6; jy++) {
12547 jj = p[jy] - 1;
12548 y[jy + 6 * jj] = 1.0;
12549 for (j = jy + 1; j < 7; j++) {
12550 iy = (6 * jj + j) - 1;
12551 if (y[iy] != 0.0) {
12552 for (ix = j + 1; ix < 7; ix++) {
12553 c = (6 * jj + ix) - 1;
12554 y[c] -= A[((j - 1) * 6 + ix) - 1] * y[iy];
12555 }
12556 }
12557 }
12558 }
12559
12560 for (j = 0; j < 6; j++) {
12561 jj = 6 * j;
12562 for (jy = 5; jy >= 0; jy--) {
12563 kAcol = 6 * jy;
12564 iy = jy + jj;
12565 if (y[iy] != 0.0) {
12566 y[iy] /= A[jy + kAcol];
12567 j_0 = jy - 1;
12568 for (ix = 0; ix <= j_0; ix++) {
12569 c = ix + jj;
12570 y[c] -= y[iy] * A[ix + kAcol];
12571 }
12572 }
12573 }
12574 }
12575}
12576
12577static void matlabCodegenHandle_matla_astwh(ros_slros_internal_block_Subs_T *obj)
12578{
12579 if (!obj->matlabCodegenIsDeleted) {
12580 obj->matlabCodegenIsDeleted = true;
12581 }
12582}
12583
12584static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj)
12585{
12586 if (!obj->matlabCodegenIsDeleted) {
12587 obj->matlabCodegenIsDeleted = true;
12588 }
12589}
12590
12591static void matlabCodegenHandle_matlabC_ast(robotics_slmanip_internal__as_T *obj)
12592{
12593 if (!obj->matlabCodegenIsDeleted) {
12594 obj->matlabCodegenIsDeleted = true;
12595 }
12596}
12597
12598static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
12599{
12600 if (obj->isInitialized == 1) {
12601 obj->isInitialized = 2;
12602 }
12603}
12604
12605static void cartesian__SystemCore_delete_as(b_inverseKinematics_cartesian_T *obj)
12606{
12607 cartesian_tr_SystemCore_release(obj);
12608}
12609
12610static void matlabCodegenHandle_matlabCo_as(b_inverseKinematics_cartesian_T *obj)
12611{
12612 if (!obj->matlabCodegenIsDeleted) {
12613 obj->matlabCodegenIsDeleted = true;
12614 cartesian__SystemCore_delete_as(obj);
12615 }
12616}
12617
12618static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_as_T
12619 *pStruct)
12620{
12621 cartesian_trajec_emxFree_char_T(&pStruct->Type);
12622 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
12623 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12624 cartesian_trajec_emxFree_real_T(&pStruct->PositionLimitsInternal);
12625 cartesian_trajec_emxFree_real_T(&pStruct->HomePositionInternal);
12626}
12627
12628static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
12629 *pStruct)
12630{
12631 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12632 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
12633}
12634
12635static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
12636 *pStruct)
12637{
12638 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
12639}
12640
12641static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
12642 *pStruct)
12643{
12644 cartesian_trajec_emxFree_real_T(&pStruct->Limits);
12645}
12646
12647static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal__as_T
12648 *pStruct)
12649{
12650 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
12651 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
12652}
12653
12654static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
12655 *pStruct)
12656{
12657 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12658}
12659
12660static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
12661 *pStruct)
12662{
12663 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
12664}
12665
12666static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
12667 *pStruct)
12668{
12669 cartesian_trajec_emxFree_char_T(&pStruct->BodyName);
12670 cartesian_trajec_emxFree_real_T(&pStruct->ErrTemp);
12671 cartesian_trajec_emxFree_real_T(&pStruct->GradTemp);
12672}
12673
12674static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
12675 *pStruct)
12676{
12677 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintMatrix);
12678 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintBound);
12679}
12680
12681static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
12682 *pStruct)
12683{
12684 cartesian_trajec_emxFree_char_T(&pStruct->Type);
12685 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
12686}
12687
12688static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
12689 *pStruct)
12690{
12691 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12692 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
12693}
12694
12695static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
12696 *pStruct)
12697{
12698 emxFreeStruct_o_robotics_manip_(&pStruct->Base);
12699}
12700
12701static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
12702 *pStruct)
12703{
12704 emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);
12705}
12706
12707static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
12708 *pStruct)
12709{
12710 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12711 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
12712}
12713
12714static void emxFreeStruct_c_rigidBodyJoint2(c_rigidBodyJoint_cartesian__a_T
12715 *pStruct)
12716{
12717 cartesian_trajec_emxFree_char_T(&pStruct->Type);
12718}
12719
12720static void emxFreeStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
12721 *pStruct)
12722{
12723 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12724 emxFreeStruct_c_rigidBodyJoint2(&pStruct->JointInternal);
12725}
12726
12727static void emxFreeStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
12728 *pStruct)
12729{
12730 emxFreeStruct_o_robotics_mani_a(&pStruct->Base);
12731}
12732
12733static void emxFreeStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
12734 *pStruct)
12735{
12736 emxFreeStruct_p_robotics_mani_a(&pStruct->TreeInternal);
12737}
12738
12739static void emxFreeStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
12740 *pStruct)
12741{
12742 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12743 emxFreeStruct_c_rigidBodyJoint2(&pStruct->JointInternal);
12744}
12745
12746static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_Publ_T *obj)
12747{
12748 if (!obj->matlabCodegenIsDeleted) {
12749 obj->matlabCodegenIsDeleted = true;
12750 }
12751}
12752
12753static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_as_T
12754 *pStruct)
12755{
12756 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
12757 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
12758 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12759 cartesian_trajec_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);
12760 cartesian_trajec_emxInit_real_T(&pStruct->HomePositionInternal, 1);
12761}
12762
12763static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
12764 *pStruct)
12765{
12766 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12767 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
12768}
12769
12770static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
12771 *pStruct)
12772{
12773 emxInitStruct_v_robotics_manip_(&pStruct->Base);
12774}
12775
12776static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
12777 *pStruct)
12778{
12779 cartesian_trajec_emxInit_real_T(&pStruct->Limits, 2);
12780}
12781
12782static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal__as_T
12783 *pStruct)
12784{
12785 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);
12786 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);
12787}
12788
12789static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
12790 *pStruct)
12791{
12792 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12793}
12794
12795static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
12796 *pStruct)
12797{
12798 emxInitStruct_w_robotics_manip_(&pStruct->Base);
12799}
12800
12801static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
12802 *pStruct)
12803{
12804 cartesian_trajec_emxInit_char_T(&pStruct->BodyName, 2);
12805 cartesian_trajec_emxInit_real_T(&pStruct->ErrTemp, 1);
12806 cartesian_trajec_emxInit_real_T(&pStruct->GradTemp, 1);
12807}
12808
12809static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
12810 *pStruct)
12811{
12812 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintMatrix, 2);
12813 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintBound, 1);
12814}
12815
12816static void cartesia_twister_state_vector_a(uint32_T mt[625])
12817{
12818 uint32_T r;
12819 int32_T b_mti;
12820 r = 5489U;
12821 mt[0] = 5489U;
12822 for (b_mti = 0; b_mti < 623; b_mti++) {
12823 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
12824 mt[b_mti + 1] = r;
12825 }
12826
12827 mt[624] = 624U;
12828}
12829
12830static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625])
12831{
12832 memset(&state[0], 0, 625U * sizeof(uint32_T));
12833 cartesia_twister_state_vector_a(state);
12834}
12835
12836static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
12837 (v_robotics_manip_internal_Rig_T *obj)
12838{
12839 v_robotics_manip_internal_Rig_T *b_obj;
12840 int8_T msubspace_data[36];
12841 real_T poslim_data[12];
12842 emxArray_char_T_cartesian_tra_T *switch_expression;
12843 boolean_T b_bool;
12844 int32_T b_kstr;
12845 char_T b[8];
12846 char_T b_0[9];
12847 int32_T loop_ub;
12848 int8_T tmp[6];
12849 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
12850 'l', 'i', 'n', 'k' };
12851
12852 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
12853 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
12854 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
12855 0.012646079447789898 };
12856
12857 static const real_T tmp_2[36] = { 0.012583419040406959,
12858 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
12859 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
12860 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
12861 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
12862 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
12863 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
12864 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
12865 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
12866 0.0785942338762368 };
12867
12868 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12869 1 };
12870
12871 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
12872 '_', 'j', 'o', 'i', 'n', 't' };
12873
12874 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
12875
12876 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12877
12878 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12879
12880 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12881 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12882
12883 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12884 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12885
12886 int32_T exitg1;
12887 b_obj = obj;
12888 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12889 obj->NameInternal->size[0] = 1;
12890 obj->NameInternal->size[1] = 13;
12891 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12892 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
12893 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12894 }
12895
12896 obj->ParentIndex = 0.0;
12897 obj->MassInternal = 0.0785942338762368;
12898 obj->CenterOfMassInternal[0] = 0.057188;
12899 obj->CenterOfMassInternal[1] = 0.005983;
12900 obj->CenterOfMassInternal[2] = 0.05;
12901 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12902 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12903 }
12904
12905 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12906 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12907 }
12908
12909 obj->JointInternal.InTree = false;
12910 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12911 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12912 }
12913
12914 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12915 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12916 }
12917
12918 b_kstr = obj->JointInternal.NameInternal->size[0] *
12919 obj->JointInternal.NameInternal->size[1];
12920 obj->JointInternal.NameInternal->size[0] = 1;
12921 obj->JointInternal.NameInternal->size[1] = 15;
12922 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12923 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
12924 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12925 }
12926
12927 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12928 obj->JointInternal.Type->size[0] = 1;
12929 obj->JointInternal.Type->size[1] = 5;
12930 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12931 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
12932 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12933 }
12934
12935 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
12936 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12937 switch_expression->size[0] = 1;
12938 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12939 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12940 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12941 - 1;
12942 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12943 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12944 }
12945
12946 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12947 b[b_kstr] = tmp_6[b_kstr];
12948 }
12949
12950 b_bool = false;
12951 if (switch_expression->size[1] == 8) {
12952 b_kstr = 1;
12953 do {
12954 exitg1 = 0;
12955 if (b_kstr - 1 < 8) {
12956 loop_ub = b_kstr - 1;
12957 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12958 exitg1 = 1;
12959 } else {
12960 b_kstr++;
12961 }
12962 } else {
12963 b_bool = true;
12964 exitg1 = 1;
12965 }
12966 } while (exitg1 == 0);
12967 }
12968
12969 if (b_bool) {
12970 b_kstr = 0;
12971 } else {
12972 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12973 b_0[b_kstr] = tmp_7[b_kstr];
12974 }
12975
12976 b_bool = false;
12977 if (switch_expression->size[1] == 9) {
12978 b_kstr = 1;
12979 do {
12980 exitg1 = 0;
12981 if (b_kstr - 1 < 9) {
12982 loop_ub = b_kstr - 1;
12983 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
12984 exitg1 = 1;
12985 } else {
12986 b_kstr++;
12987 }
12988 } else {
12989 b_bool = true;
12990 exitg1 = 1;
12991 }
12992 } while (exitg1 == 0);
12993 }
12994
12995 if (b_bool) {
12996 b_kstr = 1;
12997 } else {
12998 b_kstr = -1;
12999 }
13000 }
13001
13002 cartesian_trajec_emxFree_char_T(&switch_expression);
13003 switch (b_kstr) {
13004 case 0:
13005 tmp[0] = 0;
13006 tmp[1] = 0;
13007 tmp[2] = 1;
13008 tmp[3] = 0;
13009 tmp[4] = 0;
13010 tmp[5] = 0;
13011 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13012 msubspace_data[b_kstr] = tmp[b_kstr];
13013 }
13014
13015 poslim_data[0] = -3.1415926535897931;
13016 poslim_data[1] = 3.1415926535897931;
13017 obj->JointInternal.VelocityNumber = 1.0;
13018 obj->JointInternal.PositionNumber = 1.0;
13019 obj->JointInternal.JointAxisInternal[0] = 0.0;
13020 obj->JointInternal.JointAxisInternal[1] = 0.0;
13021 obj->JointInternal.JointAxisInternal[2] = 1.0;
13022 break;
13023
13024 case 1:
13025 tmp[0] = 0;
13026 tmp[1] = 0;
13027 tmp[2] = 0;
13028 tmp[3] = 0;
13029 tmp[4] = 0;
13030 tmp[5] = 1;
13031 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13032 msubspace_data[b_kstr] = tmp[b_kstr];
13033 }
13034
13035 poslim_data[0] = -0.5;
13036 poslim_data[1] = 0.5;
13037 obj->JointInternal.VelocityNumber = 1.0;
13038 obj->JointInternal.PositionNumber = 1.0;
13039 obj->JointInternal.JointAxisInternal[0] = 0.0;
13040 obj->JointInternal.JointAxisInternal[1] = 0.0;
13041 obj->JointInternal.JointAxisInternal[2] = 1.0;
13042 break;
13043
13044 default:
13045 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13046 msubspace_data[b_kstr] = 0;
13047 }
13048
13049 poslim_data[0] = 0.0;
13050 poslim_data[1] = 0.0;
13051 obj->JointInternal.VelocityNumber = 0.0;
13052 obj->JointInternal.PositionNumber = 0.0;
13053 obj->JointInternal.JointAxisInternal[0] = 0.0;
13054 obj->JointInternal.JointAxisInternal[1] = 0.0;
13055 obj->JointInternal.JointAxisInternal[2] = 0.0;
13056 break;
13057 }
13058
13059 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13060 obj->JointInternal.MotionSubspace->size[1];
13061 obj->JointInternal.MotionSubspace->size[0] = 6;
13062 obj->JointInternal.MotionSubspace->size[1] = 1;
13063 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13064 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13065 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13066 }
13067
13068 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13069 obj->JointInternal.PositionLimitsInternal->size[1];
13070 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13071 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13072 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13073 b_kstr);
13074 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13075 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13076 }
13077
13078 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13079 obj->JointInternal.HomePositionInternal->size[0] = 1;
13080 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13081 b_kstr);
13082 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13083 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13084 }
13085
13086 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13087 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
13088 }
13089
13090 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13091 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
13092 }
13093
13094 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13095 obj->JointInternal.MotionSubspace->size[1];
13096 obj->JointInternal.MotionSubspace->size[0] = 6;
13097 obj->JointInternal.MotionSubspace->size[1] = 1;
13098 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13099 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13100 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
13101 }
13102
13103 obj->JointInternal.InTree = true;
13104 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13105 obj->JointInternal.PositionLimitsInternal->size[1];
13106 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13107 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13108 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13109 b_kstr);
13110 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
13111 obj->JointInternal.PositionLimitsInternal->data
13112 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
13113 obj->JointInternal.JointAxisInternal[0] = 0.0;
13114 obj->JointInternal.JointAxisInternal[1] = 0.0;
13115 obj->JointInternal.JointAxisInternal[2] = 0.0;
13116 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13117 obj->JointInternal.HomePositionInternal->size[0] = 1;
13118 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13119 b_kstr);
13120 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13121 return b_obj;
13122}
13123
13124static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
13125 (v_robotics_manip_internal_Rig_T *obj)
13126{
13127 v_robotics_manip_internal_Rig_T *b_obj;
13128 int8_T msubspace_data[36];
13129 real_T poslim_data[12];
13130 emxArray_char_T_cartesian_tra_T *switch_expression;
13131 boolean_T b_bool;
13132 int32_T b_kstr;
13133 char_T b[8];
13134 char_T b_0[9];
13135 int32_T loop_ub;
13136 int8_T tmp[6];
13137 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13138 '1' };
13139
13140 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
13141 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
13142 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
13143 0.012672078048055372 };
13144
13145 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
13146 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
13147 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
13148 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
13149 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
13150 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
13151 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
13152 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
13153
13154 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13155 1 };
13156
13157 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13158 '_', '1' };
13159
13160 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13161
13162 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13163
13164 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13165 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
13166
13167 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13168 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13169
13170 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13171 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13172 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13173
13174 int32_T exitg1;
13175 b_obj = obj;
13176 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13177 obj->NameInternal->size[0] = 1;
13178 obj->NameInternal->size[1] = 10;
13179 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13180 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13181 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13182 }
13183
13184 obj->ParentIndex = 1.0;
13185 obj->MassInternal = 0.0785942338762368;
13186 obj->CenterOfMassInternal[0] = -0.037464;
13187 obj->CenterOfMassInternal[1] = 0.04726;
13188 obj->CenterOfMassInternal[2] = 0.0;
13189 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13190 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13191 }
13192
13193 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13194 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13195 }
13196
13197 obj->JointInternal.InTree = false;
13198 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13199 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13200 }
13201
13202 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13203 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13204 }
13205
13206 b_kstr = obj->JointInternal.NameInternal->size[0] *
13207 obj->JointInternal.NameInternal->size[1];
13208 obj->JointInternal.NameInternal->size[0] = 1;
13209 obj->JointInternal.NameInternal->size[1] = 11;
13210 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13211 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13212 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13213 }
13214
13215 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13216 obj->JointInternal.Type->size[0] = 1;
13217 obj->JointInternal.Type->size[1] = 8;
13218 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13219 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13220 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13221 }
13222
13223 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13224 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13225 switch_expression->size[0] = 1;
13226 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13227 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13228 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13229 - 1;
13230 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13231 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13232 }
13233
13234 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13235 b[b_kstr] = tmp_5[b_kstr];
13236 }
13237
13238 b_bool = false;
13239 if (switch_expression->size[1] == 8) {
13240 b_kstr = 1;
13241 do {
13242 exitg1 = 0;
13243 if (b_kstr - 1 < 8) {
13244 loop_ub = b_kstr - 1;
13245 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13246 exitg1 = 1;
13247 } else {
13248 b_kstr++;
13249 }
13250 } else {
13251 b_bool = true;
13252 exitg1 = 1;
13253 }
13254 } while (exitg1 == 0);
13255 }
13256
13257 if (b_bool) {
13258 b_kstr = 0;
13259 } else {
13260 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13261 b_0[b_kstr] = tmp_6[b_kstr];
13262 }
13263
13264 b_bool = false;
13265 if (switch_expression->size[1] == 9) {
13266 b_kstr = 1;
13267 do {
13268 exitg1 = 0;
13269 if (b_kstr - 1 < 9) {
13270 loop_ub = b_kstr - 1;
13271 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13272 exitg1 = 1;
13273 } else {
13274 b_kstr++;
13275 }
13276 } else {
13277 b_bool = true;
13278 exitg1 = 1;
13279 }
13280 } while (exitg1 == 0);
13281 }
13282
13283 if (b_bool) {
13284 b_kstr = 1;
13285 } else {
13286 b_kstr = -1;
13287 }
13288 }
13289
13290 cartesian_trajec_emxFree_char_T(&switch_expression);
13291 switch (b_kstr) {
13292 case 0:
13293 tmp[0] = 0;
13294 tmp[1] = 0;
13295 tmp[2] = 1;
13296 tmp[3] = 0;
13297 tmp[4] = 0;
13298 tmp[5] = 0;
13299 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13300 msubspace_data[b_kstr] = tmp[b_kstr];
13301 }
13302
13303 poslim_data[0] = -3.1415926535897931;
13304 poslim_data[1] = 3.1415926535897931;
13305 obj->JointInternal.VelocityNumber = 1.0;
13306 obj->JointInternal.PositionNumber = 1.0;
13307 obj->JointInternal.JointAxisInternal[0] = 0.0;
13308 obj->JointInternal.JointAxisInternal[1] = 0.0;
13309 obj->JointInternal.JointAxisInternal[2] = 1.0;
13310 break;
13311
13312 case 1:
13313 tmp[0] = 0;
13314 tmp[1] = 0;
13315 tmp[2] = 0;
13316 tmp[3] = 0;
13317 tmp[4] = 0;
13318 tmp[5] = 1;
13319 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13320 msubspace_data[b_kstr] = tmp[b_kstr];
13321 }
13322
13323 poslim_data[0] = -0.5;
13324 poslim_data[1] = 0.5;
13325 obj->JointInternal.VelocityNumber = 1.0;
13326 obj->JointInternal.PositionNumber = 1.0;
13327 obj->JointInternal.JointAxisInternal[0] = 0.0;
13328 obj->JointInternal.JointAxisInternal[1] = 0.0;
13329 obj->JointInternal.JointAxisInternal[2] = 1.0;
13330 break;
13331
13332 default:
13333 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13334 msubspace_data[b_kstr] = 0;
13335 }
13336
13337 poslim_data[0] = 0.0;
13338 poslim_data[1] = 0.0;
13339 obj->JointInternal.VelocityNumber = 0.0;
13340 obj->JointInternal.PositionNumber = 0.0;
13341 obj->JointInternal.JointAxisInternal[0] = 0.0;
13342 obj->JointInternal.JointAxisInternal[1] = 0.0;
13343 obj->JointInternal.JointAxisInternal[2] = 0.0;
13344 break;
13345 }
13346
13347 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13348 obj->JointInternal.MotionSubspace->size[1];
13349 obj->JointInternal.MotionSubspace->size[0] = 6;
13350 obj->JointInternal.MotionSubspace->size[1] = 1;
13351 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13352 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13353 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13354 }
13355
13356 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13357 obj->JointInternal.PositionLimitsInternal->size[1];
13358 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13359 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13360 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13361 b_kstr);
13362 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13363 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13364 }
13365
13366 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13367 obj->JointInternal.HomePositionInternal->size[0] = 1;
13368 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13369 b_kstr);
13370 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13371 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13372 }
13373
13374 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13375 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13376 }
13377
13378 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13379 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13380 }
13381
13382 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13383 obj->JointInternal.MotionSubspace->size[1];
13384 obj->JointInternal.MotionSubspace->size[0] = 6;
13385 obj->JointInternal.MotionSubspace->size[1] = 1;
13386 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13387 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13388 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13389 }
13390
13391 obj->JointInternal.InTree = true;
13392 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13393 obj->JointInternal.PositionLimitsInternal->size[1];
13394 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13395 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13396 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13397 b_kstr);
13398 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
13399 obj->JointInternal.PositionLimitsInternal->data
13400 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
13401 obj->JointInternal.JointAxisInternal[0] = 0.0;
13402 obj->JointInternal.JointAxisInternal[1] = 0.0;
13403 obj->JointInternal.JointAxisInternal[2] = 1.0;
13404 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13405 obj->JointInternal.HomePositionInternal->size[0] = 1;
13406 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13407 b_kstr);
13408 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13409 return b_obj;
13410}
13411
13412static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
13413 (v_robotics_manip_internal_Rig_T *obj)
13414{
13415 v_robotics_manip_internal_Rig_T *b_obj;
13416 int8_T msubspace_data[36];
13417 real_T poslim_data[12];
13418 emxArray_char_T_cartesian_tra_T *switch_expression;
13419 boolean_T b_bool;
13420 int32_T b_kstr;
13421 char_T b[8];
13422 char_T b_0[9];
13423 int32_T loop_ub;
13424 int8_T tmp[6];
13425 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13426 '2' };
13427
13428 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
13429 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
13430 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
13431 0.0051581160550583753 };
13432
13433 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
13434 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
13435 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
13436 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
13437 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
13438 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
13439 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
13440 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
13441 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
13442 0.0785942338762368 };
13443
13444 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13445 1 };
13446
13447 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13448 '_', '2' };
13449
13450 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13451
13452 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13453
13454 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
13455 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
13456 0.0, 0.0, 0.0, 1.0 };
13457
13458 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13459 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13460
13461 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13462 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13463 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13464
13465 int32_T exitg1;
13466 b_obj = obj;
13467 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13468 obj->NameInternal->size[0] = 1;
13469 obj->NameInternal->size[1] = 10;
13470 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13471 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13472 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13473 }
13474
13475 obj->ParentIndex = 2.0;
13476 obj->MassInternal = 0.0785942338762368;
13477 obj->CenterOfMassInternal[0] = -0.016423;
13478 obj->CenterOfMassInternal[1] = 0.031803;
13479 obj->CenterOfMassInternal[2] = -0.045948;
13480 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13481 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13482 }
13483
13484 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13485 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13486 }
13487
13488 obj->JointInternal.InTree = false;
13489 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13490 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13491 }
13492
13493 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13494 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13495 }
13496
13497 b_kstr = obj->JointInternal.NameInternal->size[0] *
13498 obj->JointInternal.NameInternal->size[1];
13499 obj->JointInternal.NameInternal->size[0] = 1;
13500 obj->JointInternal.NameInternal->size[1] = 11;
13501 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13502 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13503 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13504 }
13505
13506 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13507 obj->JointInternal.Type->size[0] = 1;
13508 obj->JointInternal.Type->size[1] = 8;
13509 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13510 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13511 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13512 }
13513
13514 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13515 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13516 switch_expression->size[0] = 1;
13517 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13518 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13519 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13520 - 1;
13521 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13522 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13523 }
13524
13525 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13526 b[b_kstr] = tmp_5[b_kstr];
13527 }
13528
13529 b_bool = false;
13530 if (switch_expression->size[1] == 8) {
13531 b_kstr = 1;
13532 do {
13533 exitg1 = 0;
13534 if (b_kstr - 1 < 8) {
13535 loop_ub = b_kstr - 1;
13536 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13537 exitg1 = 1;
13538 } else {
13539 b_kstr++;
13540 }
13541 } else {
13542 b_bool = true;
13543 exitg1 = 1;
13544 }
13545 } while (exitg1 == 0);
13546 }
13547
13548 if (b_bool) {
13549 b_kstr = 0;
13550 } else {
13551 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13552 b_0[b_kstr] = tmp_6[b_kstr];
13553 }
13554
13555 b_bool = false;
13556 if (switch_expression->size[1] == 9) {
13557 b_kstr = 1;
13558 do {
13559 exitg1 = 0;
13560 if (b_kstr - 1 < 9) {
13561 loop_ub = b_kstr - 1;
13562 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13563 exitg1 = 1;
13564 } else {
13565 b_kstr++;
13566 }
13567 } else {
13568 b_bool = true;
13569 exitg1 = 1;
13570 }
13571 } while (exitg1 == 0);
13572 }
13573
13574 if (b_bool) {
13575 b_kstr = 1;
13576 } else {
13577 b_kstr = -1;
13578 }
13579 }
13580
13581 cartesian_trajec_emxFree_char_T(&switch_expression);
13582 switch (b_kstr) {
13583 case 0:
13584 tmp[0] = 0;
13585 tmp[1] = 0;
13586 tmp[2] = 1;
13587 tmp[3] = 0;
13588 tmp[4] = 0;
13589 tmp[5] = 0;
13590 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13591 msubspace_data[b_kstr] = tmp[b_kstr];
13592 }
13593
13594 poslim_data[0] = -3.1415926535897931;
13595 poslim_data[1] = 3.1415926535897931;
13596 obj->JointInternal.VelocityNumber = 1.0;
13597 obj->JointInternal.PositionNumber = 1.0;
13598 obj->JointInternal.JointAxisInternal[0] = 0.0;
13599 obj->JointInternal.JointAxisInternal[1] = 0.0;
13600 obj->JointInternal.JointAxisInternal[2] = 1.0;
13601 break;
13602
13603 case 1:
13604 tmp[0] = 0;
13605 tmp[1] = 0;
13606 tmp[2] = 0;
13607 tmp[3] = 0;
13608 tmp[4] = 0;
13609 tmp[5] = 1;
13610 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13611 msubspace_data[b_kstr] = tmp[b_kstr];
13612 }
13613
13614 poslim_data[0] = -0.5;
13615 poslim_data[1] = 0.5;
13616 obj->JointInternal.VelocityNumber = 1.0;
13617 obj->JointInternal.PositionNumber = 1.0;
13618 obj->JointInternal.JointAxisInternal[0] = 0.0;
13619 obj->JointInternal.JointAxisInternal[1] = 0.0;
13620 obj->JointInternal.JointAxisInternal[2] = 1.0;
13621 break;
13622
13623 default:
13624 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13625 msubspace_data[b_kstr] = 0;
13626 }
13627
13628 poslim_data[0] = 0.0;
13629 poslim_data[1] = 0.0;
13630 obj->JointInternal.VelocityNumber = 0.0;
13631 obj->JointInternal.PositionNumber = 0.0;
13632 obj->JointInternal.JointAxisInternal[0] = 0.0;
13633 obj->JointInternal.JointAxisInternal[1] = 0.0;
13634 obj->JointInternal.JointAxisInternal[2] = 0.0;
13635 break;
13636 }
13637
13638 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13639 obj->JointInternal.MotionSubspace->size[1];
13640 obj->JointInternal.MotionSubspace->size[0] = 6;
13641 obj->JointInternal.MotionSubspace->size[1] = 1;
13642 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13643 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13644 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13645 }
13646
13647 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13648 obj->JointInternal.PositionLimitsInternal->size[1];
13649 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13650 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13651 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13652 b_kstr);
13653 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13654 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13655 }
13656
13657 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13658 obj->JointInternal.HomePositionInternal->size[0] = 1;
13659 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13660 b_kstr);
13661 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13662 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13663 }
13664
13665 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13666 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13667 }
13668
13669 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13670 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13671 }
13672
13673 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13674 obj->JointInternal.MotionSubspace->size[1];
13675 obj->JointInternal.MotionSubspace->size[0] = 6;
13676 obj->JointInternal.MotionSubspace->size[1] = 1;
13677 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13678 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13679 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13680 }
13681
13682 obj->JointInternal.InTree = true;
13683 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13684 obj->JointInternal.PositionLimitsInternal->size[1];
13685 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13686 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13687 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13688 b_kstr);
13689 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
13690 obj->JointInternal.PositionLimitsInternal->data
13691 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
13692 obj->JointInternal.JointAxisInternal[0] = 0.0;
13693 obj->JointInternal.JointAxisInternal[1] = 0.0;
13694 obj->JointInternal.JointAxisInternal[2] = -1.0;
13695 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13696 obj->JointInternal.HomePositionInternal->size[0] = 1;
13697 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13698 b_kstr);
13699 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13700 return b_obj;
13701}
13702
13703static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
13704 (v_robotics_manip_internal_Rig_T *obj)
13705{
13706 v_robotics_manip_internal_Rig_T *b_obj;
13707 int8_T msubspace_data[36];
13708 real_T poslim_data[12];
13709 emxArray_char_T_cartesian_tra_T *switch_expression;
13710 boolean_T b_bool;
13711 int32_T b_kstr;
13712 char_T b[8];
13713 char_T b_0[9];
13714 int32_T loop_ub;
13715 int8_T tmp[6];
13716 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13717 '3' };
13718
13719 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
13720 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
13721 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
13722 0.012477575138803739 };
13723
13724 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
13725 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
13726 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
13727 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
13728 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
13729 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
13730 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
13731 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
13732 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
13733 0.0785942338762368 };
13734
13735 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13736 1 };
13737
13738 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13739 '_', '3' };
13740
13741 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13742
13743 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13744
13745 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
13746 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
13747 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
13748 1.0 };
13749
13750 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13751 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13752
13753 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13754 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13755 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13756
13757 int32_T exitg1;
13758 b_obj = obj;
13759 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13760 obj->NameInternal->size[0] = 1;
13761 obj->NameInternal->size[1] = 10;
13762 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13763 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13764 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13765 }
13766
13767 obj->ParentIndex = 3.0;
13768 obj->MassInternal = 0.0785942338762368;
13769 obj->CenterOfMassInternal[0] = -0.031475;
13770 obj->CenterOfMassInternal[1] = 0.0131;
13771 obj->CenterOfMassInternal[2] = 0.000326;
13772 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13773 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13774 }
13775
13776 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13777 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13778 }
13779
13780 obj->JointInternal.InTree = false;
13781 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13782 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13783 }
13784
13785 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13786 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13787 }
13788
13789 b_kstr = obj->JointInternal.NameInternal->size[0] *
13790 obj->JointInternal.NameInternal->size[1];
13791 obj->JointInternal.NameInternal->size[0] = 1;
13792 obj->JointInternal.NameInternal->size[1] = 11;
13793 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13794 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13795 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13796 }
13797
13798 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13799 obj->JointInternal.Type->size[0] = 1;
13800 obj->JointInternal.Type->size[1] = 8;
13801 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13802 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13803 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13804 }
13805
13806 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13807 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13808 switch_expression->size[0] = 1;
13809 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13810 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13811 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13812 - 1;
13813 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13814 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13815 }
13816
13817 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13818 b[b_kstr] = tmp_5[b_kstr];
13819 }
13820
13821 b_bool = false;
13822 if (switch_expression->size[1] == 8) {
13823 b_kstr = 1;
13824 do {
13825 exitg1 = 0;
13826 if (b_kstr - 1 < 8) {
13827 loop_ub = b_kstr - 1;
13828 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13829 exitg1 = 1;
13830 } else {
13831 b_kstr++;
13832 }
13833 } else {
13834 b_bool = true;
13835 exitg1 = 1;
13836 }
13837 } while (exitg1 == 0);
13838 }
13839
13840 if (b_bool) {
13841 b_kstr = 0;
13842 } else {
13843 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13844 b_0[b_kstr] = tmp_6[b_kstr];
13845 }
13846
13847 b_bool = false;
13848 if (switch_expression->size[1] == 9) {
13849 b_kstr = 1;
13850 do {
13851 exitg1 = 0;
13852 if (b_kstr - 1 < 9) {
13853 loop_ub = b_kstr - 1;
13854 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13855 exitg1 = 1;
13856 } else {
13857 b_kstr++;
13858 }
13859 } else {
13860 b_bool = true;
13861 exitg1 = 1;
13862 }
13863 } while (exitg1 == 0);
13864 }
13865
13866 if (b_bool) {
13867 b_kstr = 1;
13868 } else {
13869 b_kstr = -1;
13870 }
13871 }
13872
13873 cartesian_trajec_emxFree_char_T(&switch_expression);
13874 switch (b_kstr) {
13875 case 0:
13876 tmp[0] = 0;
13877 tmp[1] = 0;
13878 tmp[2] = 1;
13879 tmp[3] = 0;
13880 tmp[4] = 0;
13881 tmp[5] = 0;
13882 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13883 msubspace_data[b_kstr] = tmp[b_kstr];
13884 }
13885
13886 poslim_data[0] = -3.1415926535897931;
13887 poslim_data[1] = 3.1415926535897931;
13888 obj->JointInternal.VelocityNumber = 1.0;
13889 obj->JointInternal.PositionNumber = 1.0;
13890 obj->JointInternal.JointAxisInternal[0] = 0.0;
13891 obj->JointInternal.JointAxisInternal[1] = 0.0;
13892 obj->JointInternal.JointAxisInternal[2] = 1.0;
13893 break;
13894
13895 case 1:
13896 tmp[0] = 0;
13897 tmp[1] = 0;
13898 tmp[2] = 0;
13899 tmp[3] = 0;
13900 tmp[4] = 0;
13901 tmp[5] = 1;
13902 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13903 msubspace_data[b_kstr] = tmp[b_kstr];
13904 }
13905
13906 poslim_data[0] = -0.5;
13907 poslim_data[1] = 0.5;
13908 obj->JointInternal.VelocityNumber = 1.0;
13909 obj->JointInternal.PositionNumber = 1.0;
13910 obj->JointInternal.JointAxisInternal[0] = 0.0;
13911 obj->JointInternal.JointAxisInternal[1] = 0.0;
13912 obj->JointInternal.JointAxisInternal[2] = 1.0;
13913 break;
13914
13915 default:
13916 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13917 msubspace_data[b_kstr] = 0;
13918 }
13919
13920 poslim_data[0] = 0.0;
13921 poslim_data[1] = 0.0;
13922 obj->JointInternal.VelocityNumber = 0.0;
13923 obj->JointInternal.PositionNumber = 0.0;
13924 obj->JointInternal.JointAxisInternal[0] = 0.0;
13925 obj->JointInternal.JointAxisInternal[1] = 0.0;
13926 obj->JointInternal.JointAxisInternal[2] = 0.0;
13927 break;
13928 }
13929
13930 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13931 obj->JointInternal.MotionSubspace->size[1];
13932 obj->JointInternal.MotionSubspace->size[0] = 6;
13933 obj->JointInternal.MotionSubspace->size[1] = 1;
13934 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13935 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13936 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13937 }
13938
13939 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13940 obj->JointInternal.PositionLimitsInternal->size[1];
13941 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13942 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13943 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13944 b_kstr);
13945 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13946 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13947 }
13948
13949 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13950 obj->JointInternal.HomePositionInternal->size[0] = 1;
13951 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13952 b_kstr);
13953 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13954 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13955 }
13956
13957 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13958 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13959 }
13960
13961 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13962 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13963 }
13964
13965 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13966 obj->JointInternal.MotionSubspace->size[1];
13967 obj->JointInternal.MotionSubspace->size[0] = 6;
13968 obj->JointInternal.MotionSubspace->size[1] = 1;
13969 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13970 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13971 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13972 }
13973
13974 obj->JointInternal.InTree = true;
13975 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13976 obj->JointInternal.PositionLimitsInternal->size[1];
13977 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13978 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13979 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13980 b_kstr);
13981 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
13982 obj->JointInternal.PositionLimitsInternal->data
13983 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
13984 obj->JointInternal.JointAxisInternal[0] = 0.0;
13985 obj->JointInternal.JointAxisInternal[1] = 0.0;
13986 obj->JointInternal.JointAxisInternal[2] = 1.0;
13987 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13988 obj->JointInternal.HomePositionInternal->size[0] = 1;
13989 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13990 b_kstr);
13991 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13992 return b_obj;
13993}
13994
13995static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
13996 (v_robotics_manip_internal_Rig_T *obj)
13997{
13998 v_robotics_manip_internal_Rig_T *b_obj;
13999 int8_T msubspace_data[36];
14000 real_T poslim_data[12];
14001 emxArray_char_T_cartesian_tra_T *switch_expression;
14002 boolean_T b_bool;
14003 int32_T b_kstr;
14004 char_T b[8];
14005 char_T b_0[9];
14006 int32_T loop_ub;
14007 int8_T tmp[6];
14008 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14009 '4' };
14010
14011 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
14012 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
14013 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
14014 0.00016056153744711284 };
14015
14016 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
14017 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
14018 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
14019 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
14020 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
14021 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
14022 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
14023 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
14024 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
14025 0.0785942338762368 };
14026
14027 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14028 1 };
14029
14030 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14031 '_', '4' };
14032
14033 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14034
14035 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14036
14037 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
14038 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
14039 0.0, -0.268, 0.0, 1.0 };
14040
14041 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14042 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14043
14044 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14045 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14046 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14047
14048 int32_T exitg1;
14049 b_obj = obj;
14050 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14051 obj->NameInternal->size[0] = 1;
14052 obj->NameInternal->size[1] = 10;
14053 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14054 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14055 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14056 }
14057
14058 obj->ParentIndex = 4.0;
14059 obj->MassInternal = 0.0785942338762368;
14060 obj->CenterOfMassInternal[0] = 0.033909;
14061 obj->CenterOfMassInternal[1] = 0.000544;
14062 obj->CenterOfMassInternal[2] = -0.013765;
14063 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14064 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14065 }
14066
14067 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14068 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14069 }
14070
14071 obj->JointInternal.InTree = false;
14072 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14073 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14074 }
14075
14076 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14077 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14078 }
14079
14080 b_kstr = obj->JointInternal.NameInternal->size[0] *
14081 obj->JointInternal.NameInternal->size[1];
14082 obj->JointInternal.NameInternal->size[0] = 1;
14083 obj->JointInternal.NameInternal->size[1] = 11;
14084 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
14085 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14086 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14087 }
14088
14089 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14090 obj->JointInternal.Type->size[0] = 1;
14091 obj->JointInternal.Type->size[1] = 8;
14092 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
14093 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14094 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14095 }
14096
14097 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14098 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14099 switch_expression->size[0] = 1;
14100 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14101 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14102 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14103 - 1;
14104 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14105 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14106 }
14107
14108 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14109 b[b_kstr] = tmp_5[b_kstr];
14110 }
14111
14112 b_bool = false;
14113 if (switch_expression->size[1] == 8) {
14114 b_kstr = 1;
14115 do {
14116 exitg1 = 0;
14117 if (b_kstr - 1 < 8) {
14118 loop_ub = b_kstr - 1;
14119 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14120 exitg1 = 1;
14121 } else {
14122 b_kstr++;
14123 }
14124 } else {
14125 b_bool = true;
14126 exitg1 = 1;
14127 }
14128 } while (exitg1 == 0);
14129 }
14130
14131 if (b_bool) {
14132 b_kstr = 0;
14133 } else {
14134 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14135 b_0[b_kstr] = tmp_6[b_kstr];
14136 }
14137
14138 b_bool = false;
14139 if (switch_expression->size[1] == 9) {
14140 b_kstr = 1;
14141 do {
14142 exitg1 = 0;
14143 if (b_kstr - 1 < 9) {
14144 loop_ub = b_kstr - 1;
14145 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14146 exitg1 = 1;
14147 } else {
14148 b_kstr++;
14149 }
14150 } else {
14151 b_bool = true;
14152 exitg1 = 1;
14153 }
14154 } while (exitg1 == 0);
14155 }
14156
14157 if (b_bool) {
14158 b_kstr = 1;
14159 } else {
14160 b_kstr = -1;
14161 }
14162 }
14163
14164 cartesian_trajec_emxFree_char_T(&switch_expression);
14165 switch (b_kstr) {
14166 case 0:
14167 tmp[0] = 0;
14168 tmp[1] = 0;
14169 tmp[2] = 1;
14170 tmp[3] = 0;
14171 tmp[4] = 0;
14172 tmp[5] = 0;
14173 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14174 msubspace_data[b_kstr] = tmp[b_kstr];
14175 }
14176
14177 poslim_data[0] = -3.1415926535897931;
14178 poslim_data[1] = 3.1415926535897931;
14179 obj->JointInternal.VelocityNumber = 1.0;
14180 obj->JointInternal.PositionNumber = 1.0;
14181 obj->JointInternal.JointAxisInternal[0] = 0.0;
14182 obj->JointInternal.JointAxisInternal[1] = 0.0;
14183 obj->JointInternal.JointAxisInternal[2] = 1.0;
14184 break;
14185
14186 case 1:
14187 tmp[0] = 0;
14188 tmp[1] = 0;
14189 tmp[2] = 0;
14190 tmp[3] = 0;
14191 tmp[4] = 0;
14192 tmp[5] = 1;
14193 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14194 msubspace_data[b_kstr] = tmp[b_kstr];
14195 }
14196
14197 poslim_data[0] = -0.5;
14198 poslim_data[1] = 0.5;
14199 obj->JointInternal.VelocityNumber = 1.0;
14200 obj->JointInternal.PositionNumber = 1.0;
14201 obj->JointInternal.JointAxisInternal[0] = 0.0;
14202 obj->JointInternal.JointAxisInternal[1] = 0.0;
14203 obj->JointInternal.JointAxisInternal[2] = 1.0;
14204 break;
14205
14206 default:
14207 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14208 msubspace_data[b_kstr] = 0;
14209 }
14210
14211 poslim_data[0] = 0.0;
14212 poslim_data[1] = 0.0;
14213 obj->JointInternal.VelocityNumber = 0.0;
14214 obj->JointInternal.PositionNumber = 0.0;
14215 obj->JointInternal.JointAxisInternal[0] = 0.0;
14216 obj->JointInternal.JointAxisInternal[1] = 0.0;
14217 obj->JointInternal.JointAxisInternal[2] = 0.0;
14218 break;
14219 }
14220
14221 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14222 obj->JointInternal.MotionSubspace->size[1];
14223 obj->JointInternal.MotionSubspace->size[0] = 6;
14224 obj->JointInternal.MotionSubspace->size[1] = 1;
14225 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14226 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14227 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14228 }
14229
14230 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14231 obj->JointInternal.PositionLimitsInternal->size[1];
14232 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14233 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14234 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14235 b_kstr);
14236 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14237 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14238 }
14239
14240 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14241 obj->JointInternal.HomePositionInternal->size[0] = 1;
14242 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14243 b_kstr);
14244 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14245 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14246 }
14247
14248 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14249 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14250 }
14251
14252 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14253 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14254 }
14255
14256 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14257 obj->JointInternal.MotionSubspace->size[1];
14258 obj->JointInternal.MotionSubspace->size[0] = 6;
14259 obj->JointInternal.MotionSubspace->size[1] = 1;
14260 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14261 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14262 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14263 }
14264
14265 obj->JointInternal.InTree = true;
14266 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14267 obj->JointInternal.PositionLimitsInternal->size[1];
14268 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14269 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14270 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14271 b_kstr);
14272 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
14273 obj->JointInternal.PositionLimitsInternal->data
14274 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
14275 obj->JointInternal.JointAxisInternal[0] = 0.0;
14276 obj->JointInternal.JointAxisInternal[1] = 0.0;
14277 obj->JointInternal.JointAxisInternal[2] = 1.0;
14278 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14279 obj->JointInternal.HomePositionInternal->size[0] = 1;
14280 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14281 b_kstr);
14282 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14283 return b_obj;
14284}
14285
14286static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
14287 (v_robotics_manip_internal_Rig_T *obj)
14288{
14289 v_robotics_manip_internal_Rig_T *b_obj;
14290 int8_T msubspace_data[36];
14291 real_T poslim_data[12];
14292 emxArray_char_T_cartesian_tra_T *switch_expression;
14293 boolean_T b_bool;
14294 int32_T b_kstr;
14295 char_T b[8];
14296 char_T b_0[9];
14297 int32_T loop_ub;
14298 int8_T tmp[6];
14299 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14300 '5' };
14301
14302 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
14303 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
14304 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
14305 0.00013851261456175015 };
14306
14307 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
14308 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
14309 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
14310 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
14311 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
14312 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
14313 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
14314 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
14315 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
14316 0.0785942338762368 };
14317
14318 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14319 1 };
14320
14321 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14322 '_', '5' };
14323
14324 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14325
14326 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14327
14328 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14329 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14330
14331 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14332 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14333
14334 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14335 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14336 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14337
14338 int32_T exitg1;
14339 b_obj = obj;
14340 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14341 obj->NameInternal->size[0] = 1;
14342 obj->NameInternal->size[1] = 10;
14343 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14344 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14345 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14346 }
14347
14348 obj->ParentIndex = 5.0;
14349 obj->MassInternal = 0.0785942338762368;
14350 obj->CenterOfMassInternal[0] = 0.029485;
14351 obj->CenterOfMassInternal[1] = -0.000458;
14352 obj->CenterOfMassInternal[2] = 0.026739;
14353 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14354 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14355 }
14356
14357 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14358 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14359 }
14360
14361 obj->JointInternal.InTree = false;
14362 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14363 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14364 }
14365
14366 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14367 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14368 }
14369
14370 b_kstr = obj->JointInternal.NameInternal->size[0] *
14371 obj->JointInternal.NameInternal->size[1];
14372 obj->JointInternal.NameInternal->size[0] = 1;
14373 obj->JointInternal.NameInternal->size[1] = 11;
14374 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
14375 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14376 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14377 }
14378
14379 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14380 obj->JointInternal.Type->size[0] = 1;
14381 obj->JointInternal.Type->size[1] = 8;
14382 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
14383 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14384 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14385 }
14386
14387 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14388 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14389 switch_expression->size[0] = 1;
14390 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14391 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14392 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14393 - 1;
14394 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14395 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14396 }
14397
14398 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14399 b[b_kstr] = tmp_5[b_kstr];
14400 }
14401
14402 b_bool = false;
14403 if (switch_expression->size[1] == 8) {
14404 b_kstr = 1;
14405 do {
14406 exitg1 = 0;
14407 if (b_kstr - 1 < 8) {
14408 loop_ub = b_kstr - 1;
14409 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14410 exitg1 = 1;
14411 } else {
14412 b_kstr++;
14413 }
14414 } else {
14415 b_bool = true;
14416 exitg1 = 1;
14417 }
14418 } while (exitg1 == 0);
14419 }
14420
14421 if (b_bool) {
14422 b_kstr = 0;
14423 } else {
14424 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14425 b_0[b_kstr] = tmp_6[b_kstr];
14426 }
14427
14428 b_bool = false;
14429 if (switch_expression->size[1] == 9) {
14430 b_kstr = 1;
14431 do {
14432 exitg1 = 0;
14433 if (b_kstr - 1 < 9) {
14434 loop_ub = b_kstr - 1;
14435 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14436 exitg1 = 1;
14437 } else {
14438 b_kstr++;
14439 }
14440 } else {
14441 b_bool = true;
14442 exitg1 = 1;
14443 }
14444 } while (exitg1 == 0);
14445 }
14446
14447 if (b_bool) {
14448 b_kstr = 1;
14449 } else {
14450 b_kstr = -1;
14451 }
14452 }
14453
14454 cartesian_trajec_emxFree_char_T(&switch_expression);
14455 switch (b_kstr) {
14456 case 0:
14457 tmp[0] = 0;
14458 tmp[1] = 0;
14459 tmp[2] = 1;
14460 tmp[3] = 0;
14461 tmp[4] = 0;
14462 tmp[5] = 0;
14463 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14464 msubspace_data[b_kstr] = tmp[b_kstr];
14465 }
14466
14467 poslim_data[0] = -3.1415926535897931;
14468 poslim_data[1] = 3.1415926535897931;
14469 obj->JointInternal.VelocityNumber = 1.0;
14470 obj->JointInternal.PositionNumber = 1.0;
14471 obj->JointInternal.JointAxisInternal[0] = 0.0;
14472 obj->JointInternal.JointAxisInternal[1] = 0.0;
14473 obj->JointInternal.JointAxisInternal[2] = 1.0;
14474 break;
14475
14476 case 1:
14477 tmp[0] = 0;
14478 tmp[1] = 0;
14479 tmp[2] = 0;
14480 tmp[3] = 0;
14481 tmp[4] = 0;
14482 tmp[5] = 1;
14483 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14484 msubspace_data[b_kstr] = tmp[b_kstr];
14485 }
14486
14487 poslim_data[0] = -0.5;
14488 poslim_data[1] = 0.5;
14489 obj->JointInternal.VelocityNumber = 1.0;
14490 obj->JointInternal.PositionNumber = 1.0;
14491 obj->JointInternal.JointAxisInternal[0] = 0.0;
14492 obj->JointInternal.JointAxisInternal[1] = 0.0;
14493 obj->JointInternal.JointAxisInternal[2] = 1.0;
14494 break;
14495
14496 default:
14497 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14498 msubspace_data[b_kstr] = 0;
14499 }
14500
14501 poslim_data[0] = 0.0;
14502 poslim_data[1] = 0.0;
14503 obj->JointInternal.VelocityNumber = 0.0;
14504 obj->JointInternal.PositionNumber = 0.0;
14505 obj->JointInternal.JointAxisInternal[0] = 0.0;
14506 obj->JointInternal.JointAxisInternal[1] = 0.0;
14507 obj->JointInternal.JointAxisInternal[2] = 0.0;
14508 break;
14509 }
14510
14511 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14512 obj->JointInternal.MotionSubspace->size[1];
14513 obj->JointInternal.MotionSubspace->size[0] = 6;
14514 obj->JointInternal.MotionSubspace->size[1] = 1;
14515 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14516 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14517 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14518 }
14519
14520 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14521 obj->JointInternal.PositionLimitsInternal->size[1];
14522 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14523 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14524 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14525 b_kstr);
14526 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14527 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14528 }
14529
14530 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14531 obj->JointInternal.HomePositionInternal->size[0] = 1;
14532 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14533 b_kstr);
14534 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14535 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14536 }
14537
14538 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14539 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14540 }
14541
14542 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14543 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14544 }
14545
14546 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14547 obj->JointInternal.MotionSubspace->size[1];
14548 obj->JointInternal.MotionSubspace->size[0] = 6;
14549 obj->JointInternal.MotionSubspace->size[1] = 1;
14550 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14551 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14552 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14553 }
14554
14555 obj->JointInternal.InTree = true;
14556 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14557 obj->JointInternal.PositionLimitsInternal->size[1];
14558 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14559 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14560 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14561 b_kstr);
14562 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
14563 obj->JointInternal.PositionLimitsInternal->data
14564 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
14565 obj->JointInternal.JointAxisInternal[0] = 0.0;
14566 obj->JointInternal.JointAxisInternal[1] = 1.0;
14567 obj->JointInternal.JointAxisInternal[2] = 0.0;
14568 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14569 obj->JointInternal.HomePositionInternal->size[0] = 1;
14570 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14571 b_kstr);
14572 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14573 return b_obj;
14574}
14575
14576static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
14577 (v_robotics_manip_internal_Rig_T *obj)
14578{
14579 v_robotics_manip_internal_Rig_T *b_obj;
14580 int8_T msubspace_data[36];
14581 real_T poslim_data[12];
14582 emxArray_char_T_cartesian_tra_T *switch_expression;
14583 boolean_T b_bool;
14584 int32_T b_kstr;
14585 char_T b[8];
14586 char_T b_0[9];
14587 int32_T loop_ub;
14588 int8_T tmp[6];
14589 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14590 '6' };
14591
14592 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
14593 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
14594 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
14595 1.006193323459457E-5 };
14596
14597 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
14598 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
14599 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
14600 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
14601 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
14602 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
14603 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
14604 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
14605 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
14606 0.0279702497322662 };
14607
14608 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14609 1 };
14610
14611 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14612 '_', '6' };
14613
14614 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14615
14616 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14617
14618 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14619 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
14620
14621 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14622 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14623
14624 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14625 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14626 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14627
14628 int32_T exitg1;
14629 b_obj = obj;
14630 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14631 obj->NameInternal->size[0] = 1;
14632 obj->NameInternal->size[1] = 10;
14633 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14634 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14635 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14636 }
14637
14638 obj->ParentIndex = 6.0;
14639 obj->MassInternal = 0.0279702497322662;
14640 obj->CenterOfMassInternal[0] = 7.0E-6;
14641 obj->CenterOfMassInternal[1] = -0.009327;
14642 obj->CenterOfMassInternal[2] = 6.0E-6;
14643 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14644 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14645 }
14646
14647 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14648 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14649 }
14650
14651 obj->JointInternal.InTree = false;
14652 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14653 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14654 }
14655
14656 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14657 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14658 }
14659
14660 b_kstr = obj->JointInternal.NameInternal->size[0] *
14661 obj->JointInternal.NameInternal->size[1];
14662 obj->JointInternal.NameInternal->size[0] = 1;
14663 obj->JointInternal.NameInternal->size[1] = 11;
14664 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
14665 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14666 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14667 }
14668
14669 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14670 obj->JointInternal.Type->size[0] = 1;
14671 obj->JointInternal.Type->size[1] = 8;
14672 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
14673 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14674 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14675 }
14676
14677 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14678 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14679 switch_expression->size[0] = 1;
14680 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14681 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14682 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14683 - 1;
14684 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14685 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14686 }
14687
14688 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14689 b[b_kstr] = tmp_5[b_kstr];
14690 }
14691
14692 b_bool = false;
14693 if (switch_expression->size[1] == 8) {
14694 b_kstr = 1;
14695 do {
14696 exitg1 = 0;
14697 if (b_kstr - 1 < 8) {
14698 loop_ub = b_kstr - 1;
14699 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14700 exitg1 = 1;
14701 } else {
14702 b_kstr++;
14703 }
14704 } else {
14705 b_bool = true;
14706 exitg1 = 1;
14707 }
14708 } while (exitg1 == 0);
14709 }
14710
14711 if (b_bool) {
14712 b_kstr = 0;
14713 } else {
14714 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14715 b_0[b_kstr] = tmp_6[b_kstr];
14716 }
14717
14718 b_bool = false;
14719 if (switch_expression->size[1] == 9) {
14720 b_kstr = 1;
14721 do {
14722 exitg1 = 0;
14723 if (b_kstr - 1 < 9) {
14724 loop_ub = b_kstr - 1;
14725 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14726 exitg1 = 1;
14727 } else {
14728 b_kstr++;
14729 }
14730 } else {
14731 b_bool = true;
14732 exitg1 = 1;
14733 }
14734 } while (exitg1 == 0);
14735 }
14736
14737 if (b_bool) {
14738 b_kstr = 1;
14739 } else {
14740 b_kstr = -1;
14741 }
14742 }
14743
14744 cartesian_trajec_emxFree_char_T(&switch_expression);
14745 switch (b_kstr) {
14746 case 0:
14747 tmp[0] = 0;
14748 tmp[1] = 0;
14749 tmp[2] = 1;
14750 tmp[3] = 0;
14751 tmp[4] = 0;
14752 tmp[5] = 0;
14753 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14754 msubspace_data[b_kstr] = tmp[b_kstr];
14755 }
14756
14757 poslim_data[0] = -3.1415926535897931;
14758 poslim_data[1] = 3.1415926535897931;
14759 obj->JointInternal.VelocityNumber = 1.0;
14760 obj->JointInternal.PositionNumber = 1.0;
14761 obj->JointInternal.JointAxisInternal[0] = 0.0;
14762 obj->JointInternal.JointAxisInternal[1] = 0.0;
14763 obj->JointInternal.JointAxisInternal[2] = 1.0;
14764 break;
14765
14766 case 1:
14767 tmp[0] = 0;
14768 tmp[1] = 0;
14769 tmp[2] = 0;
14770 tmp[3] = 0;
14771 tmp[4] = 0;
14772 tmp[5] = 1;
14773 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14774 msubspace_data[b_kstr] = tmp[b_kstr];
14775 }
14776
14777 poslim_data[0] = -0.5;
14778 poslim_data[1] = 0.5;
14779 obj->JointInternal.VelocityNumber = 1.0;
14780 obj->JointInternal.PositionNumber = 1.0;
14781 obj->JointInternal.JointAxisInternal[0] = 0.0;
14782 obj->JointInternal.JointAxisInternal[1] = 0.0;
14783 obj->JointInternal.JointAxisInternal[2] = 1.0;
14784 break;
14785
14786 default:
14787 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14788 msubspace_data[b_kstr] = 0;
14789 }
14790
14791 poslim_data[0] = 0.0;
14792 poslim_data[1] = 0.0;
14793 obj->JointInternal.VelocityNumber = 0.0;
14794 obj->JointInternal.PositionNumber = 0.0;
14795 obj->JointInternal.JointAxisInternal[0] = 0.0;
14796 obj->JointInternal.JointAxisInternal[1] = 0.0;
14797 obj->JointInternal.JointAxisInternal[2] = 0.0;
14798 break;
14799 }
14800
14801 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14802 obj->JointInternal.MotionSubspace->size[1];
14803 obj->JointInternal.MotionSubspace->size[0] = 6;
14804 obj->JointInternal.MotionSubspace->size[1] = 1;
14805 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14806 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14807 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14808 }
14809
14810 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14811 obj->JointInternal.PositionLimitsInternal->size[1];
14812 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14813 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14814 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14815 b_kstr);
14816 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14817 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14818 }
14819
14820 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14821 obj->JointInternal.HomePositionInternal->size[0] = 1;
14822 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14823 b_kstr);
14824 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14825 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14826 }
14827
14828 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14829 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14830 }
14831
14832 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14833 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14834 }
14835
14836 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14837 obj->JointInternal.MotionSubspace->size[1];
14838 obj->JointInternal.MotionSubspace->size[0] = 6;
14839 obj->JointInternal.MotionSubspace->size[1] = 1;
14840 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14841 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14842 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14843 }
14844
14845 obj->JointInternal.InTree = true;
14846 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14847 obj->JointInternal.PositionLimitsInternal->size[1];
14848 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14849 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14850 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14851 b_kstr);
14852 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
14853 obj->JointInternal.PositionLimitsInternal->data
14854 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
14855 obj->JointInternal.JointAxisInternal[0] = 0.0;
14856 obj->JointInternal.JointAxisInternal[1] = 0.0;
14857 obj->JointInternal.JointAxisInternal[2] = 1.0;
14858 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14859 obj->JointInternal.HomePositionInternal->size[0] = 1;
14860 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14861 b_kstr);
14862 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14863 return b_obj;
14864}
14865
14866static y_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
14867 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
14868 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
14869 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
14870 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
14871 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
14872 v_robotics_manip_internal_Rig_T *iobj_7)
14873{
14874 y_robotics_manip_internal_Rig_T *b_obj;
14875 v_robotics_manip_internal_Rig_T *obj_0;
14876 int8_T msubspace_data[36];
14877 real_T poslim_data[12];
14878 int8_T b_I[9];
14879 emxArray_char_T_cartesian_tra_T *switch_expression;
14880 boolean_T b_bool;
14881 int32_T b_kstr;
14882 char_T b[8];
14883 char_T b_0[9];
14884 int32_T loop_ub;
14885 int8_T tmp[6];
14886 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14887 'e', 'e' };
14888
14889 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
14890 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
14891 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
14892
14893 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14894 1 };
14895
14896 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14897 '_', 'e', 'e' };
14898
14899 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
14900
14901 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14902
14903 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14904
14905 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14906 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14907
14908 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14909 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14910
14911 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
14912
14913 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
14914
14915 int32_T exitg1;
14916 b_obj = obj;
14917 obj->Bodies[0] = RigidBody_RigidB_astwhqf2azttxa(iobj_7);
14918 obj->Bodies[0]->Index = 1.0;
14919 obj->Bodies[1] = RigidBody_Rigid_astwhqf2azttxab(iobj_0);
14920 obj->Bodies[1]->Index = 2.0;
14921 obj->Bodies[2] = c_RigidBody_Rigid_b(iobj_1);
14922 obj->Bodies[2]->Index = 3.0;
14923 obj->Bodies[3] = c_RigidBody_Rigid_i(iobj_2);
14924 obj->Bodies[3]->Index = 4.0;
14925 obj->Bodies[4] = c_RigidBody_Rigid_l(iobj_3);
14926 obj->Bodies[4]->Index = 5.0;
14927 obj->Bodies[5] = c_RigidBody_Rigid_lh(iobj_4);
14928 obj->Bodies[5]->Index = 6.0;
14929 obj->Bodies[6] = c_RigidBody_Rigid_k(iobj_5);
14930 obj->Bodies[6]->Index = 7.0;
14931 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
14932 iobj_6->NameInternal->size[0] = 1;
14933 iobj_6->NameInternal->size[1] = 11;
14934 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
14935 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14936 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14937 }
14938
14939 iobj_6->ParentIndex = 7.0;
14940 iobj_6->MassInternal = 0.0;
14941 iobj_6->CenterOfMassInternal[0] = 0.0;
14942 iobj_6->CenterOfMassInternal[1] = 0.0;
14943 iobj_6->CenterOfMassInternal[2] = 0.0;
14944 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14945 iobj_6->InertiaInternal[b_kstr] = 0.0;
14946 }
14947
14948 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14949 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
14950 }
14951
14952 iobj_6->JointInternal.InTree = false;
14953 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14954 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
14955 }
14956
14957 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14958 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
14959 }
14960
14961 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
14962 iobj_6->JointInternal.NameInternal->size[1];
14963 iobj_6->JointInternal.NameInternal->size[0] = 1;
14964 iobj_6->JointInternal.NameInternal->size[1] = 12;
14965 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr);
14966 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
14967 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
14968 }
14969
14970 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
14971 JointInternal.Type->size[1];
14972 iobj_6->JointInternal.Type->size[0] = 1;
14973 iobj_6->JointInternal.Type->size[1] = 5;
14974 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
14975 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14976 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
14977 }
14978
14979 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14980 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14981 switch_expression->size[0] = 1;
14982 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
14983 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14984 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
14985 JointInternal.Type->size[1] - 1;
14986 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14987 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
14988 }
14989
14990 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14991 b[b_kstr] = tmp_5[b_kstr];
14992 }
14993
14994 b_bool = false;
14995 if (switch_expression->size[1] == 8) {
14996 b_kstr = 1;
14997 do {
14998 exitg1 = 0;
14999 if (b_kstr - 1 < 8) {
15000 loop_ub = b_kstr - 1;
15001 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15002 exitg1 = 1;
15003 } else {
15004 b_kstr++;
15005 }
15006 } else {
15007 b_bool = true;
15008 exitg1 = 1;
15009 }
15010 } while (exitg1 == 0);
15011 }
15012
15013 if (b_bool) {
15014 b_kstr = 0;
15015 } else {
15016 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15017 b_0[b_kstr] = tmp_6[b_kstr];
15018 }
15019
15020 b_bool = false;
15021 if (switch_expression->size[1] == 9) {
15022 b_kstr = 1;
15023 do {
15024 exitg1 = 0;
15025 if (b_kstr - 1 < 9) {
15026 loop_ub = b_kstr - 1;
15027 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15028 exitg1 = 1;
15029 } else {
15030 b_kstr++;
15031 }
15032 } else {
15033 b_bool = true;
15034 exitg1 = 1;
15035 }
15036 } while (exitg1 == 0);
15037 }
15038
15039 if (b_bool) {
15040 b_kstr = 1;
15041 } else {
15042 b_kstr = -1;
15043 }
15044 }
15045
15046 switch (b_kstr) {
15047 case 0:
15048 tmp[0] = 0;
15049 tmp[1] = 0;
15050 tmp[2] = 1;
15051 tmp[3] = 0;
15052 tmp[4] = 0;
15053 tmp[5] = 0;
15054 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15055 msubspace_data[b_kstr] = tmp[b_kstr];
15056 }
15057
15058 poslim_data[0] = -3.1415926535897931;
15059 poslim_data[1] = 3.1415926535897931;
15060 iobj_6->JointInternal.VelocityNumber = 1.0;
15061 iobj_6->JointInternal.PositionNumber = 1.0;
15062 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
15063 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
15064 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
15065 break;
15066
15067 case 1:
15068 tmp[0] = 0;
15069 tmp[1] = 0;
15070 tmp[2] = 0;
15071 tmp[3] = 0;
15072 tmp[4] = 0;
15073 tmp[5] = 1;
15074 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15075 msubspace_data[b_kstr] = tmp[b_kstr];
15076 }
15077
15078 poslim_data[0] = -0.5;
15079 poslim_data[1] = 0.5;
15080 iobj_6->JointInternal.VelocityNumber = 1.0;
15081 iobj_6->JointInternal.PositionNumber = 1.0;
15082 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
15083 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
15084 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
15085 break;
15086
15087 default:
15088 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15089 msubspace_data[b_kstr] = 0;
15090 }
15091
15092 poslim_data[0] = 0.0;
15093 poslim_data[1] = 0.0;
15094 iobj_6->JointInternal.VelocityNumber = 0.0;
15095 iobj_6->JointInternal.PositionNumber = 0.0;
15096 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
15097 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
15098 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
15099 break;
15100 }
15101
15102 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
15103 iobj_6->JointInternal.MotionSubspace->size[1];
15104 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
15105 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
15106 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
15107 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15108 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15109 }
15110
15111 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
15112 iobj_6->JointInternal.PositionLimitsInternal->size[1];
15113 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
15114 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
15115 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
15116 b_kstr);
15117 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15118 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
15119 poslim_data[b_kstr];
15120 }
15121
15122 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
15123 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
15124 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
15125 b_kstr);
15126 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15127 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
15128 }
15129
15130 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15131 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
15132 }
15133
15134 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15135 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
15136 }
15137
15138 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
15139 iobj_6->JointInternal.MotionSubspace->size[1];
15140 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
15141 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
15142 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
15143 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15144 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
15145 }
15146
15147 iobj_6->JointInternal.InTree = true;
15148 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
15149 iobj_6->JointInternal.PositionLimitsInternal->size[1];
15150 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
15151 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
15152 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
15153 b_kstr);
15154 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
15155 iobj_6->JointInternal.PositionLimitsInternal->data
15156 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
15157 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
15158 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
15159 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
15160 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
15161 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
15162 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
15163 b_kstr);
15164 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
15165 obj->Bodies[7] = iobj_6;
15166 obj->Bodies[7]->Index = 8.0;
15167 obj->NumBodies = 8.0;
15168 obj->Gravity[0] = 0.0;
15169 obj->Gravity[1] = 0.0;
15170 obj->Gravity[2] = 0.0;
15171 obj_0 = &obj->Base;
15172 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
15173 obj->Base.NameInternal->size[0] = 1;
15174 obj->Base.NameInternal->size[1] = 5;
15175 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
15176 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15177 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
15178 }
15179
15180 obj->Base.JointInternal.InTree = false;
15181 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15182 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
15183 }
15184
15185 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15186 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
15187 }
15188
15189 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
15190 obj->Base.JointInternal.NameInternal->size[1];
15191 obj->Base.JointInternal.NameInternal->size[0] = 1;
15192 obj->Base.JointInternal.NameInternal->size[1] = 9;
15193 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr);
15194 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15195 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
15196 }
15197
15198 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
15199 Base.JointInternal.Type->size[1];
15200 obj->Base.JointInternal.Type->size[0] = 1;
15201 obj->Base.JointInternal.Type->size[1] = 5;
15202 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
15203 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15204 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
15205 }
15206
15207 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15208 switch_expression->size[0] = 1;
15209 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
15210 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15211 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
15212 Base.JointInternal.Type->size[1] - 1;
15213 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15214 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
15215 }
15216
15217 b_bool = false;
15218 if (switch_expression->size[1] == 8) {
15219 b_kstr = 1;
15220 do {
15221 exitg1 = 0;
15222 if (b_kstr - 1 < 8) {
15223 loop_ub = b_kstr - 1;
15224 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15225 exitg1 = 1;
15226 } else {
15227 b_kstr++;
15228 }
15229 } else {
15230 b_bool = true;
15231 exitg1 = 1;
15232 }
15233 } while (exitg1 == 0);
15234 }
15235
15236 if (b_bool) {
15237 b_kstr = 0;
15238 } else {
15239 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15240 b_0[b_kstr] = tmp_6[b_kstr];
15241 }
15242
15243 b_bool = false;
15244 if (switch_expression->size[1] == 9) {
15245 b_kstr = 1;
15246 do {
15247 exitg1 = 0;
15248 if (b_kstr - 1 < 9) {
15249 loop_ub = b_kstr - 1;
15250 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15251 exitg1 = 1;
15252 } else {
15253 b_kstr++;
15254 }
15255 } else {
15256 b_bool = true;
15257 exitg1 = 1;
15258 }
15259 } while (exitg1 == 0);
15260 }
15261
15262 if (b_bool) {
15263 b_kstr = 1;
15264 } else {
15265 b_kstr = -1;
15266 }
15267 }
15268
15269 cartesian_trajec_emxFree_char_T(&switch_expression);
15270 switch (b_kstr) {
15271 case 0:
15272 tmp[0] = 0;
15273 tmp[1] = 0;
15274 tmp[2] = 1;
15275 tmp[3] = 0;
15276 tmp[4] = 0;
15277 tmp[5] = 0;
15278 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15279 msubspace_data[b_kstr] = tmp[b_kstr];
15280 }
15281
15282 poslim_data[0] = -3.1415926535897931;
15283 poslim_data[1] = 3.1415926535897931;
15284 obj->Base.JointInternal.VelocityNumber = 1.0;
15285 obj->Base.JointInternal.PositionNumber = 1.0;
15286 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15287 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15288 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
15289 break;
15290
15291 case 1:
15292 tmp[0] = 0;
15293 tmp[1] = 0;
15294 tmp[2] = 0;
15295 tmp[3] = 0;
15296 tmp[4] = 0;
15297 tmp[5] = 1;
15298 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15299 msubspace_data[b_kstr] = tmp[b_kstr];
15300 }
15301
15302 poslim_data[0] = -0.5;
15303 poslim_data[1] = 0.5;
15304 obj->Base.JointInternal.VelocityNumber = 1.0;
15305 obj->Base.JointInternal.PositionNumber = 1.0;
15306 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15307 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15308 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
15309 break;
15310
15311 default:
15312 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15313 msubspace_data[b_kstr] = 0;
15314 }
15315
15316 poslim_data[0] = 0.0;
15317 poslim_data[1] = 0.0;
15318 obj->Base.JointInternal.VelocityNumber = 0.0;
15319 obj->Base.JointInternal.PositionNumber = 0.0;
15320 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15321 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15322 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
15323 break;
15324 }
15325
15326 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
15327 obj->Base.JointInternal.MotionSubspace->size[1];
15328 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
15329 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
15330 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);
15331 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15332 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15333 }
15334
15335 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
15336 obj->Base.JointInternal.PositionLimitsInternal->size[1];
15337 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
15338 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
15339 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
15340 b_kstr);
15341 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15342 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
15343 poslim_data[b_kstr];
15344 }
15345
15346 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
15347 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
15348 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
15349 b_kstr);
15350 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15351 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
15352 }
15353
15354 obj->Base.Index = -1.0;
15355 obj->Base.ParentIndex = -1.0;
15356 obj->Base.MassInternal = 1.0;
15357 obj->Base.CenterOfMassInternal[0] = 0.0;
15358 obj->Base.CenterOfMassInternal[1] = 0.0;
15359 obj->Base.CenterOfMassInternal[2] = 0.0;
15360 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15361 b_I[b_kstr] = 0;
15362 }
15363
15364 b_I[0] = 1;
15365 b_I[4] = 1;
15366 b_I[8] = 1;
15367 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15368 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
15369 }
15370
15371 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15372 msubspace_data[b_kstr] = 0;
15373 }
15374
15375 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15376 msubspace_data[b_kstr + 6 * b_kstr] = 1;
15377 }
15378
15379 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15380 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
15381 }
15382
15383 return b_obj;
15384}
15385
15386static void cartesian_trajectory_plann_rand(real_T r[5])
15387{
15388 for (cartesian_trajectory_planner_B.b_k_i = 0;
15389 cartesian_trajectory_planner_B.b_k_i < 5;
15390 cartesian_trajectory_planner_B.b_k_i++) {
15391 memcpy(&cartesian_trajectory_planner_B.uv[0],
15392 &cartesian_trajectory_planner_DW.state_m[0], 625U * sizeof(uint32_T));
15393 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv,
15394 cartesian_trajectory_planner_DW.state_m,
15395 &r[cartesian_trajectory_planner_B.b_k_i]);
15396 }
15397}
15398
15399static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h
15400 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
15401{
15402 w_robotics_manip_internal_Rig_T *b_obj;
15403 int8_T msubspace_data[36];
15404 real_T poslim_data[12];
15405 emxArray_char_T_cartesian_tra_T *switch_expression;
15406 boolean_T b_bool;
15407 int32_T b_kstr;
15408 char_T b[8];
15409 char_T b_0[9];
15410 int32_T loop_ub;
15411 int8_T tmp[6];
15412 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15413 '\x01' };
15414
15415 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15416 1 };
15417
15418 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15419 '\x01', '_', 'j', 'n', 't' };
15420
15421 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15422
15423 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15424
15425 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15426
15427 int32_T exitg1;
15428 b_obj = obj;
15429 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15430 obj->NameInternal->size[0] = 1;
15431 obj->NameInternal->size[1] = 10;
15432 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15433 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15434 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15435 }
15436
15437 iobj_0->InTree = false;
15438 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15439 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15440 }
15441
15442 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15443 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15444 }
15445
15446 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15447 iobj_0->NameInternal->size[0] = 1;
15448 iobj_0->NameInternal->size[1] = 14;
15449 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15450 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15451 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15452 }
15453
15454 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15455 iobj_0->Type->size[0] = 1;
15456 iobj_0->Type->size[1] = 5;
15457 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15458 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15459 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15460 }
15461
15462 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15463 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15464 switch_expression->size[0] = 1;
15465 switch_expression->size[1] = iobj_0->Type->size[1];
15466 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15467 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15468 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15469 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15470 }
15471
15472 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15473 b[b_kstr] = tmp_4[b_kstr];
15474 }
15475
15476 b_bool = false;
15477 if (switch_expression->size[1] == 8) {
15478 b_kstr = 1;
15479 do {
15480 exitg1 = 0;
15481 if (b_kstr - 1 < 8) {
15482 loop_ub = b_kstr - 1;
15483 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15484 exitg1 = 1;
15485 } else {
15486 b_kstr++;
15487 }
15488 } else {
15489 b_bool = true;
15490 exitg1 = 1;
15491 }
15492 } while (exitg1 == 0);
15493 }
15494
15495 if (b_bool) {
15496 b_kstr = 0;
15497 } else {
15498 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15499 b_0[b_kstr] = tmp_5[b_kstr];
15500 }
15501
15502 b_bool = false;
15503 if (switch_expression->size[1] == 9) {
15504 b_kstr = 1;
15505 do {
15506 exitg1 = 0;
15507 if (b_kstr - 1 < 9) {
15508 loop_ub = b_kstr - 1;
15509 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15510 exitg1 = 1;
15511 } else {
15512 b_kstr++;
15513 }
15514 } else {
15515 b_bool = true;
15516 exitg1 = 1;
15517 }
15518 } while (exitg1 == 0);
15519 }
15520
15521 if (b_bool) {
15522 b_kstr = 1;
15523 } else {
15524 b_kstr = -1;
15525 }
15526 }
15527
15528 cartesian_trajec_emxFree_char_T(&switch_expression);
15529 switch (b_kstr) {
15530 case 0:
15531 tmp[0] = 0;
15532 tmp[1] = 0;
15533 tmp[2] = 1;
15534 tmp[3] = 0;
15535 tmp[4] = 0;
15536 tmp[5] = 0;
15537 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15538 msubspace_data[b_kstr] = tmp[b_kstr];
15539 }
15540
15541 poslim_data[0] = -3.1415926535897931;
15542 poslim_data[1] = 3.1415926535897931;
15543 iobj_0->VelocityNumber = 1.0;
15544 iobj_0->PositionNumber = 1.0;
15545 iobj_0->JointAxisInternal[0] = 0.0;
15546 iobj_0->JointAxisInternal[1] = 0.0;
15547 iobj_0->JointAxisInternal[2] = 1.0;
15548 break;
15549
15550 case 1:
15551 tmp[0] = 0;
15552 tmp[1] = 0;
15553 tmp[2] = 0;
15554 tmp[3] = 0;
15555 tmp[4] = 0;
15556 tmp[5] = 1;
15557 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15558 msubspace_data[b_kstr] = tmp[b_kstr];
15559 }
15560
15561 poslim_data[0] = -0.5;
15562 poslim_data[1] = 0.5;
15563 iobj_0->VelocityNumber = 1.0;
15564 iobj_0->PositionNumber = 1.0;
15565 iobj_0->JointAxisInternal[0] = 0.0;
15566 iobj_0->JointAxisInternal[1] = 0.0;
15567 iobj_0->JointAxisInternal[2] = 1.0;
15568 break;
15569
15570 default:
15571 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15572 msubspace_data[b_kstr] = 0;
15573 }
15574
15575 poslim_data[0] = 0.0;
15576 poslim_data[1] = 0.0;
15577 iobj_0->VelocityNumber = 0.0;
15578 iobj_0->PositionNumber = 0.0;
15579 iobj_0->JointAxisInternal[0] = 0.0;
15580 iobj_0->JointAxisInternal[1] = 0.0;
15581 iobj_0->JointAxisInternal[2] = 0.0;
15582 break;
15583 }
15584
15585 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
15586 iobj_0->MotionSubspace->size[0] = 6;
15587 iobj_0->MotionSubspace->size[1] = 1;
15588 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
15589 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15590 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15591 }
15592
15593 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
15594 iobj_0->PositionLimitsInternal->size[1];
15595 iobj_0->PositionLimitsInternal->size[0] = 1;
15596 iobj_0->PositionLimitsInternal->size[1] = 2;
15597 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
15598 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15599 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15600 }
15601
15602 b_kstr = iobj_0->HomePositionInternal->size[0];
15603 iobj_0->HomePositionInternal->size[0] = 1;
15604 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
15605 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15606 iobj_0->HomePositionInternal->data[0] = 0.0;
15607 }
15608
15609 obj->JointInternal = iobj_0;
15610 obj->Index = -1.0;
15611 obj->ParentIndex = -1.0;
15612 return b_obj;
15613}
15614
15615static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h2
15616 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
15617{
15618 w_robotics_manip_internal_Rig_T *b_obj;
15619 int8_T msubspace_data[36];
15620 real_T poslim_data[12];
15621 emxArray_char_T_cartesian_tra_T *switch_expression;
15622 boolean_T b_bool;
15623 int32_T b_kstr;
15624 char_T b[8];
15625 char_T b_0[9];
15626 int32_T loop_ub;
15627 int8_T tmp[6];
15628 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15629 '\x02' };
15630
15631 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15632 1 };
15633
15634 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15635 '\x02', '_', 'j', 'n', 't' };
15636
15637 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15638
15639 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15640
15641 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15642
15643 int32_T exitg1;
15644 b_obj = obj;
15645 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15646 obj->NameInternal->size[0] = 1;
15647 obj->NameInternal->size[1] = 10;
15648 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15649 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15650 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15651 }
15652
15653 iobj_0->InTree = false;
15654 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15655 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15656 }
15657
15658 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15659 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15660 }
15661
15662 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15663 iobj_0->NameInternal->size[0] = 1;
15664 iobj_0->NameInternal->size[1] = 14;
15665 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15666 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15667 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15668 }
15669
15670 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15671 iobj_0->Type->size[0] = 1;
15672 iobj_0->Type->size[1] = 5;
15673 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15674 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15675 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15676 }
15677
15678 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15679 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15680 switch_expression->size[0] = 1;
15681 switch_expression->size[1] = iobj_0->Type->size[1];
15682 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15683 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15684 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15685 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15686 }
15687
15688 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15689 b[b_kstr] = tmp_4[b_kstr];
15690 }
15691
15692 b_bool = false;
15693 if (switch_expression->size[1] == 8) {
15694 b_kstr = 1;
15695 do {
15696 exitg1 = 0;
15697 if (b_kstr - 1 < 8) {
15698 loop_ub = b_kstr - 1;
15699 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15700 exitg1 = 1;
15701 } else {
15702 b_kstr++;
15703 }
15704 } else {
15705 b_bool = true;
15706 exitg1 = 1;
15707 }
15708 } while (exitg1 == 0);
15709 }
15710
15711 if (b_bool) {
15712 b_kstr = 0;
15713 } else {
15714 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15715 b_0[b_kstr] = tmp_5[b_kstr];
15716 }
15717
15718 b_bool = false;
15719 if (switch_expression->size[1] == 9) {
15720 b_kstr = 1;
15721 do {
15722 exitg1 = 0;
15723 if (b_kstr - 1 < 9) {
15724 loop_ub = b_kstr - 1;
15725 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15726 exitg1 = 1;
15727 } else {
15728 b_kstr++;
15729 }
15730 } else {
15731 b_bool = true;
15732 exitg1 = 1;
15733 }
15734 } while (exitg1 == 0);
15735 }
15736
15737 if (b_bool) {
15738 b_kstr = 1;
15739 } else {
15740 b_kstr = -1;
15741 }
15742 }
15743
15744 cartesian_trajec_emxFree_char_T(&switch_expression);
15745 switch (b_kstr) {
15746 case 0:
15747 tmp[0] = 0;
15748 tmp[1] = 0;
15749 tmp[2] = 1;
15750 tmp[3] = 0;
15751 tmp[4] = 0;
15752 tmp[5] = 0;
15753 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15754 msubspace_data[b_kstr] = tmp[b_kstr];
15755 }
15756
15757 poslim_data[0] = -3.1415926535897931;
15758 poslim_data[1] = 3.1415926535897931;
15759 iobj_0->VelocityNumber = 1.0;
15760 iobj_0->PositionNumber = 1.0;
15761 iobj_0->JointAxisInternal[0] = 0.0;
15762 iobj_0->JointAxisInternal[1] = 0.0;
15763 iobj_0->JointAxisInternal[2] = 1.0;
15764 break;
15765
15766 case 1:
15767 tmp[0] = 0;
15768 tmp[1] = 0;
15769 tmp[2] = 0;
15770 tmp[3] = 0;
15771 tmp[4] = 0;
15772 tmp[5] = 1;
15773 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15774 msubspace_data[b_kstr] = tmp[b_kstr];
15775 }
15776
15777 poslim_data[0] = -0.5;
15778 poslim_data[1] = 0.5;
15779 iobj_0->VelocityNumber = 1.0;
15780 iobj_0->PositionNumber = 1.0;
15781 iobj_0->JointAxisInternal[0] = 0.0;
15782 iobj_0->JointAxisInternal[1] = 0.0;
15783 iobj_0->JointAxisInternal[2] = 1.0;
15784 break;
15785
15786 default:
15787 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15788 msubspace_data[b_kstr] = 0;
15789 }
15790
15791 poslim_data[0] = 0.0;
15792 poslim_data[1] = 0.0;
15793 iobj_0->VelocityNumber = 0.0;
15794 iobj_0->PositionNumber = 0.0;
15795 iobj_0->JointAxisInternal[0] = 0.0;
15796 iobj_0->JointAxisInternal[1] = 0.0;
15797 iobj_0->JointAxisInternal[2] = 0.0;
15798 break;
15799 }
15800
15801 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
15802 iobj_0->MotionSubspace->size[0] = 6;
15803 iobj_0->MotionSubspace->size[1] = 1;
15804 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
15805 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15806 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15807 }
15808
15809 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
15810 iobj_0->PositionLimitsInternal->size[1];
15811 iobj_0->PositionLimitsInternal->size[0] = 1;
15812 iobj_0->PositionLimitsInternal->size[1] = 2;
15813 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
15814 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15815 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15816 }
15817
15818 b_kstr = iobj_0->HomePositionInternal->size[0];
15819 iobj_0->HomePositionInternal->size[0] = 1;
15820 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
15821 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15822 iobj_0->HomePositionInternal->data[0] = 0.0;
15823 }
15824
15825 obj->JointInternal = iobj_0;
15826 obj->Index = -1.0;
15827 obj->ParentIndex = -1.0;
15828 return b_obj;
15829}
15830
15831static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_m
15832 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
15833{
15834 w_robotics_manip_internal_Rig_T *b_obj;
15835 int8_T msubspace_data[36];
15836 real_T poslim_data[12];
15837 emxArray_char_T_cartesian_tra_T *switch_expression;
15838 boolean_T b_bool;
15839 int32_T b_kstr;
15840 char_T b[8];
15841 char_T b_0[9];
15842 int32_T loop_ub;
15843 int8_T tmp[6];
15844 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15845 '\x03' };
15846
15847 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15848 1 };
15849
15850 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15851 '\x03', '_', 'j', 'n', 't' };
15852
15853 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15854
15855 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15856
15857 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15858
15859 int32_T exitg1;
15860 b_obj = obj;
15861 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15862 obj->NameInternal->size[0] = 1;
15863 obj->NameInternal->size[1] = 10;
15864 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15865 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15866 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15867 }
15868
15869 iobj_0->InTree = false;
15870 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15871 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15872 }
15873
15874 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15875 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15876 }
15877
15878 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15879 iobj_0->NameInternal->size[0] = 1;
15880 iobj_0->NameInternal->size[1] = 14;
15881 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15882 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15883 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15884 }
15885
15886 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15887 iobj_0->Type->size[0] = 1;
15888 iobj_0->Type->size[1] = 5;
15889 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15890 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15891 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15892 }
15893
15894 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15895 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15896 switch_expression->size[0] = 1;
15897 switch_expression->size[1] = iobj_0->Type->size[1];
15898 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15899 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15900 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15901 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15902 }
15903
15904 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15905 b[b_kstr] = tmp_4[b_kstr];
15906 }
15907
15908 b_bool = false;
15909 if (switch_expression->size[1] == 8) {
15910 b_kstr = 1;
15911 do {
15912 exitg1 = 0;
15913 if (b_kstr - 1 < 8) {
15914 loop_ub = b_kstr - 1;
15915 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15916 exitg1 = 1;
15917 } else {
15918 b_kstr++;
15919 }
15920 } else {
15921 b_bool = true;
15922 exitg1 = 1;
15923 }
15924 } while (exitg1 == 0);
15925 }
15926
15927 if (b_bool) {
15928 b_kstr = 0;
15929 } else {
15930 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15931 b_0[b_kstr] = tmp_5[b_kstr];
15932 }
15933
15934 b_bool = false;
15935 if (switch_expression->size[1] == 9) {
15936 b_kstr = 1;
15937 do {
15938 exitg1 = 0;
15939 if (b_kstr - 1 < 9) {
15940 loop_ub = b_kstr - 1;
15941 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15942 exitg1 = 1;
15943 } else {
15944 b_kstr++;
15945 }
15946 } else {
15947 b_bool = true;
15948 exitg1 = 1;
15949 }
15950 } while (exitg1 == 0);
15951 }
15952
15953 if (b_bool) {
15954 b_kstr = 1;
15955 } else {
15956 b_kstr = -1;
15957 }
15958 }
15959
15960 cartesian_trajec_emxFree_char_T(&switch_expression);
15961 switch (b_kstr) {
15962 case 0:
15963 tmp[0] = 0;
15964 tmp[1] = 0;
15965 tmp[2] = 1;
15966 tmp[3] = 0;
15967 tmp[4] = 0;
15968 tmp[5] = 0;
15969 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15970 msubspace_data[b_kstr] = tmp[b_kstr];
15971 }
15972
15973 poslim_data[0] = -3.1415926535897931;
15974 poslim_data[1] = 3.1415926535897931;
15975 iobj_0->VelocityNumber = 1.0;
15976 iobj_0->PositionNumber = 1.0;
15977 iobj_0->JointAxisInternal[0] = 0.0;
15978 iobj_0->JointAxisInternal[1] = 0.0;
15979 iobj_0->JointAxisInternal[2] = 1.0;
15980 break;
15981
15982 case 1:
15983 tmp[0] = 0;
15984 tmp[1] = 0;
15985 tmp[2] = 0;
15986 tmp[3] = 0;
15987 tmp[4] = 0;
15988 tmp[5] = 1;
15989 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15990 msubspace_data[b_kstr] = tmp[b_kstr];
15991 }
15992
15993 poslim_data[0] = -0.5;
15994 poslim_data[1] = 0.5;
15995 iobj_0->VelocityNumber = 1.0;
15996 iobj_0->PositionNumber = 1.0;
15997 iobj_0->JointAxisInternal[0] = 0.0;
15998 iobj_0->JointAxisInternal[1] = 0.0;
15999 iobj_0->JointAxisInternal[2] = 1.0;
16000 break;
16001
16002 default:
16003 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16004 msubspace_data[b_kstr] = 0;
16005 }
16006
16007 poslim_data[0] = 0.0;
16008 poslim_data[1] = 0.0;
16009 iobj_0->VelocityNumber = 0.0;
16010 iobj_0->PositionNumber = 0.0;
16011 iobj_0->JointAxisInternal[0] = 0.0;
16012 iobj_0->JointAxisInternal[1] = 0.0;
16013 iobj_0->JointAxisInternal[2] = 0.0;
16014 break;
16015 }
16016
16017 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16018 iobj_0->MotionSubspace->size[0] = 6;
16019 iobj_0->MotionSubspace->size[1] = 1;
16020 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16021 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16022 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16023 }
16024
16025 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
16026 iobj_0->PositionLimitsInternal->size[1];
16027 iobj_0->PositionLimitsInternal->size[0] = 1;
16028 iobj_0->PositionLimitsInternal->size[1] = 2;
16029 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
16030 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16031 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16032 }
16033
16034 b_kstr = iobj_0->HomePositionInternal->size[0];
16035 iobj_0->HomePositionInternal->size[0] = 1;
16036 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
16037 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16038 iobj_0->HomePositionInternal->data[0] = 0.0;
16039 }
16040
16041 obj->JointInternal = iobj_0;
16042 obj->Index = -1.0;
16043 obj->ParentIndex = -1.0;
16044 return b_obj;
16045}
16046
16047static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_o
16048 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
16049{
16050 w_robotics_manip_internal_Rig_T *b_obj;
16051 int8_T msubspace_data[36];
16052 real_T poslim_data[12];
16053 emxArray_char_T_cartesian_tra_T *switch_expression;
16054 boolean_T b_bool;
16055 int32_T b_kstr;
16056 char_T b[8];
16057 char_T b_0[9];
16058 int32_T loop_ub;
16059 int8_T tmp[6];
16060 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16061 '\x04' };
16062
16063 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16064 1 };
16065
16066 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16067 '\x04', '_', 'j', 'n', 't' };
16068
16069 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
16070
16071 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16072
16073 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16074
16075 int32_T exitg1;
16076 b_obj = obj;
16077 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16078 obj->NameInternal->size[0] = 1;
16079 obj->NameInternal->size[1] = 10;
16080 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16081 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16082 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16083 }
16084
16085 iobj_0->InTree = false;
16086 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16087 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
16088 }
16089
16090 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16091 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
16092 }
16093
16094 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
16095 iobj_0->NameInternal->size[0] = 1;
16096 iobj_0->NameInternal->size[1] = 14;
16097 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
16098 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
16099 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
16100 }
16101
16102 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
16103 iobj_0->Type->size[0] = 1;
16104 iobj_0->Type->size[1] = 5;
16105 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
16106 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16107 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
16108 }
16109
16110 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
16111 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16112 switch_expression->size[0] = 1;
16113 switch_expression->size[1] = iobj_0->Type->size[1];
16114 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16115 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
16116 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16117 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
16118 }
16119
16120 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16121 b[b_kstr] = tmp_4[b_kstr];
16122 }
16123
16124 b_bool = false;
16125 if (switch_expression->size[1] == 8) {
16126 b_kstr = 1;
16127 do {
16128 exitg1 = 0;
16129 if (b_kstr - 1 < 8) {
16130 loop_ub = b_kstr - 1;
16131 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16132 exitg1 = 1;
16133 } else {
16134 b_kstr++;
16135 }
16136 } else {
16137 b_bool = true;
16138 exitg1 = 1;
16139 }
16140 } while (exitg1 == 0);
16141 }
16142
16143 if (b_bool) {
16144 b_kstr = 0;
16145 } else {
16146 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16147 b_0[b_kstr] = tmp_5[b_kstr];
16148 }
16149
16150 b_bool = false;
16151 if (switch_expression->size[1] == 9) {
16152 b_kstr = 1;
16153 do {
16154 exitg1 = 0;
16155 if (b_kstr - 1 < 9) {
16156 loop_ub = b_kstr - 1;
16157 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16158 exitg1 = 1;
16159 } else {
16160 b_kstr++;
16161 }
16162 } else {
16163 b_bool = true;
16164 exitg1 = 1;
16165 }
16166 } while (exitg1 == 0);
16167 }
16168
16169 if (b_bool) {
16170 b_kstr = 1;
16171 } else {
16172 b_kstr = -1;
16173 }
16174 }
16175
16176 cartesian_trajec_emxFree_char_T(&switch_expression);
16177 switch (b_kstr) {
16178 case 0:
16179 tmp[0] = 0;
16180 tmp[1] = 0;
16181 tmp[2] = 1;
16182 tmp[3] = 0;
16183 tmp[4] = 0;
16184 tmp[5] = 0;
16185 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16186 msubspace_data[b_kstr] = tmp[b_kstr];
16187 }
16188
16189 poslim_data[0] = -3.1415926535897931;
16190 poslim_data[1] = 3.1415926535897931;
16191 iobj_0->VelocityNumber = 1.0;
16192 iobj_0->PositionNumber = 1.0;
16193 iobj_0->JointAxisInternal[0] = 0.0;
16194 iobj_0->JointAxisInternal[1] = 0.0;
16195 iobj_0->JointAxisInternal[2] = 1.0;
16196 break;
16197
16198 case 1:
16199 tmp[0] = 0;
16200 tmp[1] = 0;
16201 tmp[2] = 0;
16202 tmp[3] = 0;
16203 tmp[4] = 0;
16204 tmp[5] = 1;
16205 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16206 msubspace_data[b_kstr] = tmp[b_kstr];
16207 }
16208
16209 poslim_data[0] = -0.5;
16210 poslim_data[1] = 0.5;
16211 iobj_0->VelocityNumber = 1.0;
16212 iobj_0->PositionNumber = 1.0;
16213 iobj_0->JointAxisInternal[0] = 0.0;
16214 iobj_0->JointAxisInternal[1] = 0.0;
16215 iobj_0->JointAxisInternal[2] = 1.0;
16216 break;
16217
16218 default:
16219 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16220 msubspace_data[b_kstr] = 0;
16221 }
16222
16223 poslim_data[0] = 0.0;
16224 poslim_data[1] = 0.0;
16225 iobj_0->VelocityNumber = 0.0;
16226 iobj_0->PositionNumber = 0.0;
16227 iobj_0->JointAxisInternal[0] = 0.0;
16228 iobj_0->JointAxisInternal[1] = 0.0;
16229 iobj_0->JointAxisInternal[2] = 0.0;
16230 break;
16231 }
16232
16233 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16234 iobj_0->MotionSubspace->size[0] = 6;
16235 iobj_0->MotionSubspace->size[1] = 1;
16236 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16237 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16238 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16239 }
16240
16241 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
16242 iobj_0->PositionLimitsInternal->size[1];
16243 iobj_0->PositionLimitsInternal->size[0] = 1;
16244 iobj_0->PositionLimitsInternal->size[1] = 2;
16245 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
16246 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16247 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16248 }
16249
16250 b_kstr = iobj_0->HomePositionInternal->size[0];
16251 iobj_0->HomePositionInternal->size[0] = 1;
16252 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
16253 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16254 iobj_0->HomePositionInternal->data[0] = 0.0;
16255 }
16256
16257 obj->JointInternal = iobj_0;
16258 obj->Index = -1.0;
16259 obj->ParentIndex = -1.0;
16260 return b_obj;
16261}
16262
16263static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_d
16264 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
16265{
16266 w_robotics_manip_internal_Rig_T *b_obj;
16267 int8_T msubspace_data[36];
16268 real_T poslim_data[12];
16269 emxArray_char_T_cartesian_tra_T *switch_expression;
16270 boolean_T b_bool;
16271 int32_T b_kstr;
16272 char_T b[8];
16273 char_T b_0[9];
16274 int32_T loop_ub;
16275 int8_T tmp[6];
16276 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16277 '\x05' };
16278
16279 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16280 1 };
16281
16282 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16283 '\x05', '_', 'j', 'n', 't' };
16284
16285 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
16286
16287 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16288
16289 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16290
16291 int32_T exitg1;
16292 b_obj = obj;
16293 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16294 obj->NameInternal->size[0] = 1;
16295 obj->NameInternal->size[1] = 10;
16296 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16297 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16298 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16299 }
16300
16301 iobj_0->InTree = false;
16302 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16303 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
16304 }
16305
16306 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16307 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
16308 }
16309
16310 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
16311 iobj_0->NameInternal->size[0] = 1;
16312 iobj_0->NameInternal->size[1] = 14;
16313 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
16314 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
16315 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
16316 }
16317
16318 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
16319 iobj_0->Type->size[0] = 1;
16320 iobj_0->Type->size[1] = 5;
16321 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
16322 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16323 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
16324 }
16325
16326 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
16327 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16328 switch_expression->size[0] = 1;
16329 switch_expression->size[1] = iobj_0->Type->size[1];
16330 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16331 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
16332 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16333 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
16334 }
16335
16336 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16337 b[b_kstr] = tmp_4[b_kstr];
16338 }
16339
16340 b_bool = false;
16341 if (switch_expression->size[1] == 8) {
16342 b_kstr = 1;
16343 do {
16344 exitg1 = 0;
16345 if (b_kstr - 1 < 8) {
16346 loop_ub = b_kstr - 1;
16347 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16348 exitg1 = 1;
16349 } else {
16350 b_kstr++;
16351 }
16352 } else {
16353 b_bool = true;
16354 exitg1 = 1;
16355 }
16356 } while (exitg1 == 0);
16357 }
16358
16359 if (b_bool) {
16360 b_kstr = 0;
16361 } else {
16362 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16363 b_0[b_kstr] = tmp_5[b_kstr];
16364 }
16365
16366 b_bool = false;
16367 if (switch_expression->size[1] == 9) {
16368 b_kstr = 1;
16369 do {
16370 exitg1 = 0;
16371 if (b_kstr - 1 < 9) {
16372 loop_ub = b_kstr - 1;
16373 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16374 exitg1 = 1;
16375 } else {
16376 b_kstr++;
16377 }
16378 } else {
16379 b_bool = true;
16380 exitg1 = 1;
16381 }
16382 } while (exitg1 == 0);
16383 }
16384
16385 if (b_bool) {
16386 b_kstr = 1;
16387 } else {
16388 b_kstr = -1;
16389 }
16390 }
16391
16392 cartesian_trajec_emxFree_char_T(&switch_expression);
16393 switch (b_kstr) {
16394 case 0:
16395 tmp[0] = 0;
16396 tmp[1] = 0;
16397 tmp[2] = 1;
16398 tmp[3] = 0;
16399 tmp[4] = 0;
16400 tmp[5] = 0;
16401 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16402 msubspace_data[b_kstr] = tmp[b_kstr];
16403 }
16404
16405 poslim_data[0] = -3.1415926535897931;
16406 poslim_data[1] = 3.1415926535897931;
16407 iobj_0->VelocityNumber = 1.0;
16408 iobj_0->PositionNumber = 1.0;
16409 iobj_0->JointAxisInternal[0] = 0.0;
16410 iobj_0->JointAxisInternal[1] = 0.0;
16411 iobj_0->JointAxisInternal[2] = 1.0;
16412 break;
16413
16414 case 1:
16415 tmp[0] = 0;
16416 tmp[1] = 0;
16417 tmp[2] = 0;
16418 tmp[3] = 0;
16419 tmp[4] = 0;
16420 tmp[5] = 1;
16421 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16422 msubspace_data[b_kstr] = tmp[b_kstr];
16423 }
16424
16425 poslim_data[0] = -0.5;
16426 poslim_data[1] = 0.5;
16427 iobj_0->VelocityNumber = 1.0;
16428 iobj_0->PositionNumber = 1.0;
16429 iobj_0->JointAxisInternal[0] = 0.0;
16430 iobj_0->JointAxisInternal[1] = 0.0;
16431 iobj_0->JointAxisInternal[2] = 1.0;
16432 break;
16433
16434 default:
16435 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16436 msubspace_data[b_kstr] = 0;
16437 }
16438
16439 poslim_data[0] = 0.0;
16440 poslim_data[1] = 0.0;
16441 iobj_0->VelocityNumber = 0.0;
16442 iobj_0->PositionNumber = 0.0;
16443 iobj_0->JointAxisInternal[0] = 0.0;
16444 iobj_0->JointAxisInternal[1] = 0.0;
16445 iobj_0->JointAxisInternal[2] = 0.0;
16446 break;
16447 }
16448
16449 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16450 iobj_0->MotionSubspace->size[0] = 6;
16451 iobj_0->MotionSubspace->size[1] = 1;
16452 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16453 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16454 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16455 }
16456
16457 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
16458 iobj_0->PositionLimitsInternal->size[1];
16459 iobj_0->PositionLimitsInternal->size[0] = 1;
16460 iobj_0->PositionLimitsInternal->size[1] = 2;
16461 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
16462 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16463 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16464 }
16465
16466 b_kstr = iobj_0->HomePositionInternal->size[0];
16467 iobj_0->HomePositionInternal->size[0] = 1;
16468 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
16469 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16470 iobj_0->HomePositionInternal->data[0] = 0.0;
16471 }
16472
16473 obj->JointInternal = iobj_0;
16474 obj->Index = -1.0;
16475 obj->ParentIndex = -1.0;
16476 return b_obj;
16477}
16478
16479static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
16480 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
16481 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
16482 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
16483 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
16484 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_as_T
16485 *iobj_7, c_rigidBodyJoint_cartesian_as_T *iobj_8,
16486 c_rigidBodyJoint_cartesian_as_T *iobj_9, c_rigidBodyJoint_cartesian_as_T
16487 *iobj_10, c_rigidBodyJoint_cartesian_as_T *iobj_11,
16488 c_rigidBodyJoint_cartesian_as_T *iobj_12, c_rigidBodyJoint_cartesian_as_T
16489 *iobj_13, c_rigidBodyJoint_cartesian_as_T *iobj_14,
16490 w_robotics_manip_internal_Rig_T *iobj_15)
16491{
16492 emxArray_char_T_cartesian_tra_T *switch_expression;
16493 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16494 '\x06' };
16495
16496 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16497 1 };
16498
16499 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16500 '\x06', '_', 'j', 'n', 't' };
16501
16502 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
16503
16504 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16505
16506 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16507
16508 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16509 '\x07' };
16510
16511 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16512 '\x07', '_', 'j', 'n', 't' };
16513
16514 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16515 '\x08' };
16516
16517 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16518 '\x08', '_', 'j', 'n', 't' };
16519
16520 int32_T exitg1;
16521 obj->Bodies[0] = c_RigidBody_Rigid_h(iobj_15, iobj_14);
16522 obj->Bodies[1] = c_RigidBody_Rigid_h2(iobj_0, iobj_7);
16523 obj->Bodies[2] = c_RigidBody_Rigid_m(iobj_1, iobj_8);
16524 obj->Bodies[3] = c_RigidBody_Rigid_o(iobj_2, iobj_9);
16525 obj->Bodies[4] = c_RigidBody_Rigid_d(iobj_3, iobj_10);
16526 cartesian_trajectory_planner_B.b_kstr_l = iobj_4->NameInternal->size[0] *
16527 iobj_4->NameInternal->size[1];
16528 iobj_4->NameInternal->size[0] = 1;
16529 iobj_4->NameInternal->size[1] = 10;
16530 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal,
16531 cartesian_trajectory_planner_B.b_kstr_l);
16532 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16533 cartesian_trajectory_planner_B.b_kstr_l < 10;
16534 cartesian_trajectory_planner_B.b_kstr_l++) {
16535 iobj_4->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
16536 tmp[cartesian_trajectory_planner_B.b_kstr_l];
16537 }
16538
16539 iobj_11->InTree = false;
16540 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16541 cartesian_trajectory_planner_B.b_kstr_l < 16;
16542 cartesian_trajectory_planner_B.b_kstr_l++) {
16543 iobj_11->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_l] =
16544 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
16545 }
16546
16547 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16548 cartesian_trajectory_planner_B.b_kstr_l < 16;
16549 cartesian_trajectory_planner_B.b_kstr_l++) {
16550 iobj_11->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_l] =
16551 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
16552 }
16553
16554 cartesian_trajectory_planner_B.b_kstr_l = iobj_11->NameInternal->size[0] *
16555 iobj_11->NameInternal->size[1];
16556 iobj_11->NameInternal->size[0] = 1;
16557 iobj_11->NameInternal->size[1] = 14;
16558 cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal,
16559 cartesian_trajectory_planner_B.b_kstr_l);
16560 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16561 cartesian_trajectory_planner_B.b_kstr_l < 14;
16562 cartesian_trajectory_planner_B.b_kstr_l++) {
16563 iobj_11->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
16564 tmp_1[cartesian_trajectory_planner_B.b_kstr_l];
16565 }
16566
16567 cartesian_trajectory_planner_B.b_kstr_l = iobj_11->Type->size[0] *
16568 iobj_11->Type->size[1];
16569 iobj_11->Type->size[0] = 1;
16570 iobj_11->Type->size[1] = 5;
16571 cartes_emxEnsureCapacity_char_T(iobj_11->Type,
16572 cartesian_trajectory_planner_B.b_kstr_l);
16573 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16574 cartesian_trajectory_planner_B.b_kstr_l < 5;
16575 cartesian_trajectory_planner_B.b_kstr_l++) {
16576 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_l] =
16577 tmp_2[cartesian_trajectory_planner_B.b_kstr_l];
16578 }
16579
16580 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
16581 cartesian_trajectory_planner_B.b_kstr_l = switch_expression->size[0] *
16582 switch_expression->size[1];
16583 switch_expression->size[0] = 1;
16584 switch_expression->size[1] = iobj_11->Type->size[1];
16585 cartes_emxEnsureCapacity_char_T(switch_expression,
16586 cartesian_trajectory_planner_B.b_kstr_l);
16587 cartesian_trajectory_planner_B.loop_ub_j = iobj_11->Type->size[0] *
16588 iobj_11->Type->size[1] - 1;
16589 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16590 cartesian_trajectory_planner_B.b_kstr_l <=
16591 cartesian_trajectory_planner_B.loop_ub_j;
16592 cartesian_trajectory_planner_B.b_kstr_l++) {
16593 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_l] =
16594 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_l];
16595 }
16596
16597 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16598 cartesian_trajectory_planner_B.b_kstr_l < 8;
16599 cartesian_trajectory_planner_B.b_kstr_l++) {
16600 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.b_kstr_l] =
16601 tmp_3[cartesian_trajectory_planner_B.b_kstr_l];
16602 }
16603
16604 cartesian_trajectory_planner_B.b_bool_o = false;
16605 if (switch_expression->size[1] == 8) {
16606 cartesian_trajectory_planner_B.b_kstr_l = 1;
16607 do {
16608 exitg1 = 0;
16609 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 8) {
16610 cartesian_trajectory_planner_B.loop_ub_j =
16611 cartesian_trajectory_planner_B.b_kstr_l - 1;
16612 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j] !=
16613 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.loop_ub_j])
16614 {
16615 exitg1 = 1;
16616 } else {
16617 cartesian_trajectory_planner_B.b_kstr_l++;
16618 }
16619 } else {
16620 cartesian_trajectory_planner_B.b_bool_o = true;
16621 exitg1 = 1;
16622 }
16623 } while (exitg1 == 0);
16624 }
16625
16626 if (cartesian_trajectory_planner_B.b_bool_o) {
16627 cartesian_trajectory_planner_B.b_kstr_l = 0;
16628 } else {
16629 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16630 cartesian_trajectory_planner_B.b_kstr_l < 9;
16631 cartesian_trajectory_planner_B.b_kstr_l++) {
16632 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.b_kstr_l]
16633 = tmp_4[cartesian_trajectory_planner_B.b_kstr_l];
16634 }
16635
16636 cartesian_trajectory_planner_B.b_bool_o = false;
16637 if (switch_expression->size[1] == 9) {
16638 cartesian_trajectory_planner_B.b_kstr_l = 1;
16639 do {
16640 exitg1 = 0;
16641 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 9) {
16642 cartesian_trajectory_planner_B.loop_ub_j =
16643 cartesian_trajectory_planner_B.b_kstr_l - 1;
16644 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j]
16645 !=
16646 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.loop_ub_j])
16647 {
16648 exitg1 = 1;
16649 } else {
16650 cartesian_trajectory_planner_B.b_kstr_l++;
16651 }
16652 } else {
16653 cartesian_trajectory_planner_B.b_bool_o = true;
16654 exitg1 = 1;
16655 }
16656 } while (exitg1 == 0);
16657 }
16658
16659 if (cartesian_trajectory_planner_B.b_bool_o) {
16660 cartesian_trajectory_planner_B.b_kstr_l = 1;
16661 } else {
16662 cartesian_trajectory_planner_B.b_kstr_l = -1;
16663 }
16664 }
16665
16666 switch (cartesian_trajectory_planner_B.b_kstr_l) {
16667 case 0:
16668 cartesian_trajectory_planner_B.iv1[0] = 0;
16669 cartesian_trajectory_planner_B.iv1[1] = 0;
16670 cartesian_trajectory_planner_B.iv1[2] = 1;
16671 cartesian_trajectory_planner_B.iv1[3] = 0;
16672 cartesian_trajectory_planner_B.iv1[4] = 0;
16673 cartesian_trajectory_planner_B.iv1[5] = 0;
16674 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16675 cartesian_trajectory_planner_B.b_kstr_l < 6;
16676 cartesian_trajectory_planner_B.b_kstr_l++) {
16677 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16678 =
16679 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
16680 }
16681
16682 cartesian_trajectory_planner_B.poslim_data_o[0] = -3.1415926535897931;
16683 cartesian_trajectory_planner_B.poslim_data_o[1] = 3.1415926535897931;
16684 iobj_11->VelocityNumber = 1.0;
16685 iobj_11->PositionNumber = 1.0;
16686 iobj_11->JointAxisInternal[0] = 0.0;
16687 iobj_11->JointAxisInternal[1] = 0.0;
16688 iobj_11->JointAxisInternal[2] = 1.0;
16689 break;
16690
16691 case 1:
16692 cartesian_trajectory_planner_B.iv1[0] = 0;
16693 cartesian_trajectory_planner_B.iv1[1] = 0;
16694 cartesian_trajectory_planner_B.iv1[2] = 0;
16695 cartesian_trajectory_planner_B.iv1[3] = 0;
16696 cartesian_trajectory_planner_B.iv1[4] = 0;
16697 cartesian_trajectory_planner_B.iv1[5] = 1;
16698 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16699 cartesian_trajectory_planner_B.b_kstr_l < 6;
16700 cartesian_trajectory_planner_B.b_kstr_l++) {
16701 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16702 =
16703 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
16704 }
16705
16706 cartesian_trajectory_planner_B.poslim_data_o[0] = -0.5;
16707 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.5;
16708 iobj_11->VelocityNumber = 1.0;
16709 iobj_11->PositionNumber = 1.0;
16710 iobj_11->JointAxisInternal[0] = 0.0;
16711 iobj_11->JointAxisInternal[1] = 0.0;
16712 iobj_11->JointAxisInternal[2] = 1.0;
16713 break;
16714
16715 default:
16716 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16717 cartesian_trajectory_planner_B.b_kstr_l < 6;
16718 cartesian_trajectory_planner_B.b_kstr_l++) {
16719 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16720 = 0;
16721 }
16722
16723 cartesian_trajectory_planner_B.poslim_data_o[0] = 0.0;
16724 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.0;
16725 iobj_11->VelocityNumber = 0.0;
16726 iobj_11->PositionNumber = 0.0;
16727 iobj_11->JointAxisInternal[0] = 0.0;
16728 iobj_11->JointAxisInternal[1] = 0.0;
16729 iobj_11->JointAxisInternal[2] = 0.0;
16730 break;
16731 }
16732
16733 cartesian_trajectory_planner_B.b_kstr_l = iobj_11->MotionSubspace->size[0] *
16734 iobj_11->MotionSubspace->size[1];
16735 iobj_11->MotionSubspace->size[0] = 6;
16736 iobj_11->MotionSubspace->size[1] = 1;
16737 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,
16738 cartesian_trajectory_planner_B.b_kstr_l);
16739 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16740 cartesian_trajectory_planner_B.b_kstr_l < 6;
16741 cartesian_trajectory_planner_B.b_kstr_l++) {
16742 iobj_11->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_l] =
16743 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l];
16744 }
16745
16746 cartesian_trajectory_planner_B.b_kstr_l = iobj_11->
16747 PositionLimitsInternal->size[0] * iobj_11->PositionLimitsInternal->size[1];
16748 iobj_11->PositionLimitsInternal->size[0] = 1;
16749 iobj_11->PositionLimitsInternal->size[1] = 2;
16750 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
16751 cartesian_trajectory_planner_B.b_kstr_l);
16752 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16753 cartesian_trajectory_planner_B.b_kstr_l < 2;
16754 cartesian_trajectory_planner_B.b_kstr_l++) {
16755 iobj_11->PositionLimitsInternal->
16756 data[cartesian_trajectory_planner_B.b_kstr_l] =
16757 cartesian_trajectory_planner_B.poslim_data_o[cartesian_trajectory_planner_B.b_kstr_l];
16758 }
16759
16760 cartesian_trajectory_planner_B.b_kstr_l = iobj_11->HomePositionInternal->size
16761 [0];
16762 iobj_11->HomePositionInternal->size[0] = 1;
16763 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
16764 cartesian_trajectory_planner_B.b_kstr_l);
16765 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16766 cartesian_trajectory_planner_B.b_kstr_l < 1;
16767 cartesian_trajectory_planner_B.b_kstr_l++) {
16768 iobj_11->HomePositionInternal->data[0] = 0.0;
16769 }
16770
16771 iobj_4->JointInternal = iobj_11;
16772 iobj_4->Index = -1.0;
16773 iobj_4->ParentIndex = -1.0;
16774 obj->Bodies[5] = iobj_4;
16775 cartesian_trajectory_planner_B.b_kstr_l = iobj_5->NameInternal->size[0] *
16776 iobj_5->NameInternal->size[1];
16777 iobj_5->NameInternal->size[0] = 1;
16778 iobj_5->NameInternal->size[1] = 10;
16779 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal,
16780 cartesian_trajectory_planner_B.b_kstr_l);
16781 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16782 cartesian_trajectory_planner_B.b_kstr_l < 10;
16783 cartesian_trajectory_planner_B.b_kstr_l++) {
16784 iobj_5->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
16785 tmp_5[cartesian_trajectory_planner_B.b_kstr_l];
16786 }
16787
16788 iobj_12->InTree = false;
16789 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16790 cartesian_trajectory_planner_B.b_kstr_l < 16;
16791 cartesian_trajectory_planner_B.b_kstr_l++) {
16792 iobj_12->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_l] =
16793 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
16794 }
16795
16796 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16797 cartesian_trajectory_planner_B.b_kstr_l < 16;
16798 cartesian_trajectory_planner_B.b_kstr_l++) {
16799 iobj_12->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_l] =
16800 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
16801 }
16802
16803 cartesian_trajectory_planner_B.b_kstr_l = iobj_12->NameInternal->size[0] *
16804 iobj_12->NameInternal->size[1];
16805 iobj_12->NameInternal->size[0] = 1;
16806 iobj_12->NameInternal->size[1] = 14;
16807 cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal,
16808 cartesian_trajectory_planner_B.b_kstr_l);
16809 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16810 cartesian_trajectory_planner_B.b_kstr_l < 14;
16811 cartesian_trajectory_planner_B.b_kstr_l++) {
16812 iobj_12->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
16813 tmp_6[cartesian_trajectory_planner_B.b_kstr_l];
16814 }
16815
16816 cartesian_trajectory_planner_B.b_kstr_l = iobj_12->Type->size[0] *
16817 iobj_12->Type->size[1];
16818 iobj_12->Type->size[0] = 1;
16819 iobj_12->Type->size[1] = 5;
16820 cartes_emxEnsureCapacity_char_T(iobj_12->Type,
16821 cartesian_trajectory_planner_B.b_kstr_l);
16822 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16823 cartesian_trajectory_planner_B.b_kstr_l < 5;
16824 cartesian_trajectory_planner_B.b_kstr_l++) {
16825 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_l] =
16826 tmp_2[cartesian_trajectory_planner_B.b_kstr_l];
16827 }
16828
16829 cartesian_trajectory_planner_B.b_kstr_l = switch_expression->size[0] *
16830 switch_expression->size[1];
16831 switch_expression->size[0] = 1;
16832 switch_expression->size[1] = iobj_12->Type->size[1];
16833 cartes_emxEnsureCapacity_char_T(switch_expression,
16834 cartesian_trajectory_planner_B.b_kstr_l);
16835 cartesian_trajectory_planner_B.loop_ub_j = iobj_12->Type->size[0] *
16836 iobj_12->Type->size[1] - 1;
16837 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16838 cartesian_trajectory_planner_B.b_kstr_l <=
16839 cartesian_trajectory_planner_B.loop_ub_j;
16840 cartesian_trajectory_planner_B.b_kstr_l++) {
16841 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_l] =
16842 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_l];
16843 }
16844
16845 cartesian_trajectory_planner_B.b_bool_o = false;
16846 if (switch_expression->size[1] == 8) {
16847 cartesian_trajectory_planner_B.b_kstr_l = 1;
16848 do {
16849 exitg1 = 0;
16850 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 8) {
16851 cartesian_trajectory_planner_B.loop_ub_j =
16852 cartesian_trajectory_planner_B.b_kstr_l - 1;
16853 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j] !=
16854 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.loop_ub_j])
16855 {
16856 exitg1 = 1;
16857 } else {
16858 cartesian_trajectory_planner_B.b_kstr_l++;
16859 }
16860 } else {
16861 cartesian_trajectory_planner_B.b_bool_o = true;
16862 exitg1 = 1;
16863 }
16864 } while (exitg1 == 0);
16865 }
16866
16867 if (cartesian_trajectory_planner_B.b_bool_o) {
16868 cartesian_trajectory_planner_B.b_kstr_l = 0;
16869 } else {
16870 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16871 cartesian_trajectory_planner_B.b_kstr_l < 9;
16872 cartesian_trajectory_planner_B.b_kstr_l++) {
16873 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.b_kstr_l]
16874 = tmp_4[cartesian_trajectory_planner_B.b_kstr_l];
16875 }
16876
16877 cartesian_trajectory_planner_B.b_bool_o = false;
16878 if (switch_expression->size[1] == 9) {
16879 cartesian_trajectory_planner_B.b_kstr_l = 1;
16880 do {
16881 exitg1 = 0;
16882 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 9) {
16883 cartesian_trajectory_planner_B.loop_ub_j =
16884 cartesian_trajectory_planner_B.b_kstr_l - 1;
16885 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j]
16886 !=
16887 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.loop_ub_j])
16888 {
16889 exitg1 = 1;
16890 } else {
16891 cartesian_trajectory_planner_B.b_kstr_l++;
16892 }
16893 } else {
16894 cartesian_trajectory_planner_B.b_bool_o = true;
16895 exitg1 = 1;
16896 }
16897 } while (exitg1 == 0);
16898 }
16899
16900 if (cartesian_trajectory_planner_B.b_bool_o) {
16901 cartesian_trajectory_planner_B.b_kstr_l = 1;
16902 } else {
16903 cartesian_trajectory_planner_B.b_kstr_l = -1;
16904 }
16905 }
16906
16907 switch (cartesian_trajectory_planner_B.b_kstr_l) {
16908 case 0:
16909 cartesian_trajectory_planner_B.iv1[0] = 0;
16910 cartesian_trajectory_planner_B.iv1[1] = 0;
16911 cartesian_trajectory_planner_B.iv1[2] = 1;
16912 cartesian_trajectory_planner_B.iv1[3] = 0;
16913 cartesian_trajectory_planner_B.iv1[4] = 0;
16914 cartesian_trajectory_planner_B.iv1[5] = 0;
16915 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16916 cartesian_trajectory_planner_B.b_kstr_l < 6;
16917 cartesian_trajectory_planner_B.b_kstr_l++) {
16918 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16919 =
16920 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
16921 }
16922
16923 cartesian_trajectory_planner_B.poslim_data_o[0] = -3.1415926535897931;
16924 cartesian_trajectory_planner_B.poslim_data_o[1] = 3.1415926535897931;
16925 iobj_12->VelocityNumber = 1.0;
16926 iobj_12->PositionNumber = 1.0;
16927 iobj_12->JointAxisInternal[0] = 0.0;
16928 iobj_12->JointAxisInternal[1] = 0.0;
16929 iobj_12->JointAxisInternal[2] = 1.0;
16930 break;
16931
16932 case 1:
16933 cartesian_trajectory_planner_B.iv1[0] = 0;
16934 cartesian_trajectory_planner_B.iv1[1] = 0;
16935 cartesian_trajectory_planner_B.iv1[2] = 0;
16936 cartesian_trajectory_planner_B.iv1[3] = 0;
16937 cartesian_trajectory_planner_B.iv1[4] = 0;
16938 cartesian_trajectory_planner_B.iv1[5] = 1;
16939 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16940 cartesian_trajectory_planner_B.b_kstr_l < 6;
16941 cartesian_trajectory_planner_B.b_kstr_l++) {
16942 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16943 =
16944 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
16945 }
16946
16947 cartesian_trajectory_planner_B.poslim_data_o[0] = -0.5;
16948 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.5;
16949 iobj_12->VelocityNumber = 1.0;
16950 iobj_12->PositionNumber = 1.0;
16951 iobj_12->JointAxisInternal[0] = 0.0;
16952 iobj_12->JointAxisInternal[1] = 0.0;
16953 iobj_12->JointAxisInternal[2] = 1.0;
16954 break;
16955
16956 default:
16957 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16958 cartesian_trajectory_planner_B.b_kstr_l < 6;
16959 cartesian_trajectory_planner_B.b_kstr_l++) {
16960 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
16961 = 0;
16962 }
16963
16964 cartesian_trajectory_planner_B.poslim_data_o[0] = 0.0;
16965 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.0;
16966 iobj_12->VelocityNumber = 0.0;
16967 iobj_12->PositionNumber = 0.0;
16968 iobj_12->JointAxisInternal[0] = 0.0;
16969 iobj_12->JointAxisInternal[1] = 0.0;
16970 iobj_12->JointAxisInternal[2] = 0.0;
16971 break;
16972 }
16973
16974 cartesian_trajectory_planner_B.b_kstr_l = iobj_12->MotionSubspace->size[0] *
16975 iobj_12->MotionSubspace->size[1];
16976 iobj_12->MotionSubspace->size[0] = 6;
16977 iobj_12->MotionSubspace->size[1] = 1;
16978 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,
16979 cartesian_trajectory_planner_B.b_kstr_l);
16980 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16981 cartesian_trajectory_planner_B.b_kstr_l < 6;
16982 cartesian_trajectory_planner_B.b_kstr_l++) {
16983 iobj_12->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_l] =
16984 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l];
16985 }
16986
16987 cartesian_trajectory_planner_B.b_kstr_l = iobj_12->
16988 PositionLimitsInternal->size[0] * iobj_12->PositionLimitsInternal->size[1];
16989 iobj_12->PositionLimitsInternal->size[0] = 1;
16990 iobj_12->PositionLimitsInternal->size[1] = 2;
16991 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
16992 cartesian_trajectory_planner_B.b_kstr_l);
16993 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
16994 cartesian_trajectory_planner_B.b_kstr_l < 2;
16995 cartesian_trajectory_planner_B.b_kstr_l++) {
16996 iobj_12->PositionLimitsInternal->
16997 data[cartesian_trajectory_planner_B.b_kstr_l] =
16998 cartesian_trajectory_planner_B.poslim_data_o[cartesian_trajectory_planner_B.b_kstr_l];
16999 }
17000
17001 cartesian_trajectory_planner_B.b_kstr_l = iobj_12->HomePositionInternal->size
17002 [0];
17003 iobj_12->HomePositionInternal->size[0] = 1;
17004 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
17005 cartesian_trajectory_planner_B.b_kstr_l);
17006 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17007 cartesian_trajectory_planner_B.b_kstr_l < 1;
17008 cartesian_trajectory_planner_B.b_kstr_l++) {
17009 iobj_12->HomePositionInternal->data[0] = 0.0;
17010 }
17011
17012 iobj_5->JointInternal = iobj_12;
17013 iobj_5->Index = -1.0;
17014 iobj_5->ParentIndex = -1.0;
17015 obj->Bodies[6] = iobj_5;
17016 cartesian_trajectory_planner_B.b_kstr_l = iobj_6->NameInternal->size[0] *
17017 iobj_6->NameInternal->size[1];
17018 iobj_6->NameInternal->size[0] = 1;
17019 iobj_6->NameInternal->size[1] = 10;
17020 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal,
17021 cartesian_trajectory_planner_B.b_kstr_l);
17022 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17023 cartesian_trajectory_planner_B.b_kstr_l < 10;
17024 cartesian_trajectory_planner_B.b_kstr_l++) {
17025 iobj_6->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
17026 tmp_7[cartesian_trajectory_planner_B.b_kstr_l];
17027 }
17028
17029 iobj_13->InTree = false;
17030 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17031 cartesian_trajectory_planner_B.b_kstr_l < 16;
17032 cartesian_trajectory_planner_B.b_kstr_l++) {
17033 iobj_13->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_l] =
17034 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
17035 }
17036
17037 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17038 cartesian_trajectory_planner_B.b_kstr_l < 16;
17039 cartesian_trajectory_planner_B.b_kstr_l++) {
17040 iobj_13->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_l] =
17041 tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
17042 }
17043
17044 cartesian_trajectory_planner_B.b_kstr_l = iobj_13->NameInternal->size[0] *
17045 iobj_13->NameInternal->size[1];
17046 iobj_13->NameInternal->size[0] = 1;
17047 iobj_13->NameInternal->size[1] = 14;
17048 cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal,
17049 cartesian_trajectory_planner_B.b_kstr_l);
17050 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17051 cartesian_trajectory_planner_B.b_kstr_l < 14;
17052 cartesian_trajectory_planner_B.b_kstr_l++) {
17053 iobj_13->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_l] =
17054 tmp_8[cartesian_trajectory_planner_B.b_kstr_l];
17055 }
17056
17057 cartesian_trajectory_planner_B.b_kstr_l = iobj_13->Type->size[0] *
17058 iobj_13->Type->size[1];
17059 iobj_13->Type->size[0] = 1;
17060 iobj_13->Type->size[1] = 5;
17061 cartes_emxEnsureCapacity_char_T(iobj_13->Type,
17062 cartesian_trajectory_planner_B.b_kstr_l);
17063 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17064 cartesian_trajectory_planner_B.b_kstr_l < 5;
17065 cartesian_trajectory_planner_B.b_kstr_l++) {
17066 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_l] =
17067 tmp_2[cartesian_trajectory_planner_B.b_kstr_l];
17068 }
17069
17070 cartesian_trajectory_planner_B.b_kstr_l = switch_expression->size[0] *
17071 switch_expression->size[1];
17072 switch_expression->size[0] = 1;
17073 switch_expression->size[1] = iobj_13->Type->size[1];
17074 cartes_emxEnsureCapacity_char_T(switch_expression,
17075 cartesian_trajectory_planner_B.b_kstr_l);
17076 cartesian_trajectory_planner_B.loop_ub_j = iobj_13->Type->size[0] *
17077 iobj_13->Type->size[1] - 1;
17078 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17079 cartesian_trajectory_planner_B.b_kstr_l <=
17080 cartesian_trajectory_planner_B.loop_ub_j;
17081 cartesian_trajectory_planner_B.b_kstr_l++) {
17082 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_l] =
17083 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_l];
17084 }
17085
17086 cartesian_trajectory_planner_B.b_bool_o = false;
17087 if (switch_expression->size[1] == 8) {
17088 cartesian_trajectory_planner_B.b_kstr_l = 1;
17089 do {
17090 exitg1 = 0;
17091 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 8) {
17092 cartesian_trajectory_planner_B.loop_ub_j =
17093 cartesian_trajectory_planner_B.b_kstr_l - 1;
17094 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j] !=
17095 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.loop_ub_j])
17096 {
17097 exitg1 = 1;
17098 } else {
17099 cartesian_trajectory_planner_B.b_kstr_l++;
17100 }
17101 } else {
17102 cartesian_trajectory_planner_B.b_bool_o = true;
17103 exitg1 = 1;
17104 }
17105 } while (exitg1 == 0);
17106 }
17107
17108 if (cartesian_trajectory_planner_B.b_bool_o) {
17109 cartesian_trajectory_planner_B.b_kstr_l = 0;
17110 } else {
17111 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17112 cartesian_trajectory_planner_B.b_kstr_l < 9;
17113 cartesian_trajectory_planner_B.b_kstr_l++) {
17114 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.b_kstr_l]
17115 = tmp_4[cartesian_trajectory_planner_B.b_kstr_l];
17116 }
17117
17118 cartesian_trajectory_planner_B.b_bool_o = false;
17119 if (switch_expression->size[1] == 9) {
17120 cartesian_trajectory_planner_B.b_kstr_l = 1;
17121 do {
17122 exitg1 = 0;
17123 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 9) {
17124 cartesian_trajectory_planner_B.loop_ub_j =
17125 cartesian_trajectory_planner_B.b_kstr_l - 1;
17126 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_j]
17127 !=
17128 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.loop_ub_j])
17129 {
17130 exitg1 = 1;
17131 } else {
17132 cartesian_trajectory_planner_B.b_kstr_l++;
17133 }
17134 } else {
17135 cartesian_trajectory_planner_B.b_bool_o = true;
17136 exitg1 = 1;
17137 }
17138 } while (exitg1 == 0);
17139 }
17140
17141 if (cartesian_trajectory_planner_B.b_bool_o) {
17142 cartesian_trajectory_planner_B.b_kstr_l = 1;
17143 } else {
17144 cartesian_trajectory_planner_B.b_kstr_l = -1;
17145 }
17146 }
17147
17148 cartesian_trajec_emxFree_char_T(&switch_expression);
17149 switch (cartesian_trajectory_planner_B.b_kstr_l) {
17150 case 0:
17151 cartesian_trajectory_planner_B.iv1[0] = 0;
17152 cartesian_trajectory_planner_B.iv1[1] = 0;
17153 cartesian_trajectory_planner_B.iv1[2] = 1;
17154 cartesian_trajectory_planner_B.iv1[3] = 0;
17155 cartesian_trajectory_planner_B.iv1[4] = 0;
17156 cartesian_trajectory_planner_B.iv1[5] = 0;
17157 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17158 cartesian_trajectory_planner_B.b_kstr_l < 6;
17159 cartesian_trajectory_planner_B.b_kstr_l++) {
17160 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
17161 =
17162 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
17163 }
17164
17165 cartesian_trajectory_planner_B.poslim_data_o[0] = -3.1415926535897931;
17166 cartesian_trajectory_planner_B.poslim_data_o[1] = 3.1415926535897931;
17167 iobj_13->VelocityNumber = 1.0;
17168 iobj_13->PositionNumber = 1.0;
17169 iobj_13->JointAxisInternal[0] = 0.0;
17170 iobj_13->JointAxisInternal[1] = 0.0;
17171 iobj_13->JointAxisInternal[2] = 1.0;
17172 break;
17173
17174 case 1:
17175 cartesian_trajectory_planner_B.iv1[0] = 0;
17176 cartesian_trajectory_planner_B.iv1[1] = 0;
17177 cartesian_trajectory_planner_B.iv1[2] = 0;
17178 cartesian_trajectory_planner_B.iv1[3] = 0;
17179 cartesian_trajectory_planner_B.iv1[4] = 0;
17180 cartesian_trajectory_planner_B.iv1[5] = 1;
17181 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17182 cartesian_trajectory_planner_B.b_kstr_l < 6;
17183 cartesian_trajectory_planner_B.b_kstr_l++) {
17184 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
17185 =
17186 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_l];
17187 }
17188
17189 cartesian_trajectory_planner_B.poslim_data_o[0] = -0.5;
17190 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.5;
17191 iobj_13->VelocityNumber = 1.0;
17192 iobj_13->PositionNumber = 1.0;
17193 iobj_13->JointAxisInternal[0] = 0.0;
17194 iobj_13->JointAxisInternal[1] = 0.0;
17195 iobj_13->JointAxisInternal[2] = 1.0;
17196 break;
17197
17198 default:
17199 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17200 cartesian_trajectory_planner_B.b_kstr_l < 6;
17201 cartesian_trajectory_planner_B.b_kstr_l++) {
17202 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l]
17203 = 0;
17204 }
17205
17206 cartesian_trajectory_planner_B.poslim_data_o[0] = 0.0;
17207 cartesian_trajectory_planner_B.poslim_data_o[1] = 0.0;
17208 iobj_13->VelocityNumber = 0.0;
17209 iobj_13->PositionNumber = 0.0;
17210 iobj_13->JointAxisInternal[0] = 0.0;
17211 iobj_13->JointAxisInternal[1] = 0.0;
17212 iobj_13->JointAxisInternal[2] = 0.0;
17213 break;
17214 }
17215
17216 cartesian_trajectory_planner_B.b_kstr_l = iobj_13->MotionSubspace->size[0] *
17217 iobj_13->MotionSubspace->size[1];
17218 iobj_13->MotionSubspace->size[0] = 6;
17219 iobj_13->MotionSubspace->size[1] = 1;
17220 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,
17221 cartesian_trajectory_planner_B.b_kstr_l);
17222 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17223 cartesian_trajectory_planner_B.b_kstr_l < 6;
17224 cartesian_trajectory_planner_B.b_kstr_l++) {
17225 iobj_13->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_l] =
17226 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.b_kstr_l];
17227 }
17228
17229 cartesian_trajectory_planner_B.b_kstr_l = iobj_13->
17230 PositionLimitsInternal->size[0] * iobj_13->PositionLimitsInternal->size[1];
17231 iobj_13->PositionLimitsInternal->size[0] = 1;
17232 iobj_13->PositionLimitsInternal->size[1] = 2;
17233 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
17234 cartesian_trajectory_planner_B.b_kstr_l);
17235 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17236 cartesian_trajectory_planner_B.b_kstr_l < 2;
17237 cartesian_trajectory_planner_B.b_kstr_l++) {
17238 iobj_13->PositionLimitsInternal->
17239 data[cartesian_trajectory_planner_B.b_kstr_l] =
17240 cartesian_trajectory_planner_B.poslim_data_o[cartesian_trajectory_planner_B.b_kstr_l];
17241 }
17242
17243 cartesian_trajectory_planner_B.b_kstr_l = iobj_13->HomePositionInternal->size
17244 [0];
17245 iobj_13->HomePositionInternal->size[0] = 1;
17246 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
17247 cartesian_trajectory_planner_B.b_kstr_l);
17248 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17249 cartesian_trajectory_planner_B.b_kstr_l < 1;
17250 cartesian_trajectory_planner_B.b_kstr_l++) {
17251 iobj_13->HomePositionInternal->data[0] = 0.0;
17252 }
17253
17254 iobj_6->JointInternal = iobj_13;
17255 iobj_6->Index = -1.0;
17256 iobj_6->ParentIndex = -1.0;
17257 obj->Bodies[7] = iobj_6;
17258 obj->NumBodies = 0.0;
17259 obj->NumNonFixedBodies = 0.0;
17260 obj->PositionNumber = 0.0;
17261 obj->VelocityNumber = 0.0;
17262 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr_h);
17263 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17264 cartesian_trajectory_planner_B.b_kstr_l < 8;
17265 cartesian_trajectory_planner_B.b_kstr_l++) {
17266 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_l] = 0.0;
17267 }
17268
17269 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17270 cartesian_trajectory_planner_B.b_kstr_l < 8;
17271 cartesian_trajectory_planner_B.b_kstr_l++) {
17272 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_l + 8] = -1.0;
17273 }
17274
17275 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17276 cartesian_trajectory_planner_B.b_kstr_l < 8;
17277 cartesian_trajectory_planner_B.b_kstr_l++) {
17278 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_l] = 0.0;
17279 }
17280
17281 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
17282 cartesian_trajectory_planner_B.b_kstr_l < 8;
17283 cartesian_trajectory_planner_B.b_kstr_l++) {
17284 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_l + 8] = -1.0;
17285 }
17286}
17287
17288static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ast
17289 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
17290 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
17291 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
17292 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
17293 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
17294 c_rigidBodyJoint_cartesian_as_T *iobj_7, c_rigidBodyJoint_cartesian_as_T
17295 *iobj_8, c_rigidBodyJoint_cartesian_as_T *iobj_9,
17296 c_rigidBodyJoint_cartesian_as_T *iobj_10, c_rigidBodyJoint_cartesian_as_T
17297 *iobj_11, c_rigidBodyJoint_cartesian_as_T *iobj_12,
17298 c_rigidBodyJoint_cartesian_as_T *iobj_13, c_rigidBodyJoint_cartesian_as_T
17299 *iobj_14, c_rigidBodyJoint_cartesian_as_T *iobj_15,
17300 w_robotics_manip_internal_Rig_T *iobj_16)
17301{
17302 x_robotics_manip_internal_Rig_T *b_obj;
17303 emxArray_char_T_cartesian_tra_T *switch_expression;
17304 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
17305 };
17306
17307 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
17308
17309 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
17310
17311 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17312
17313 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17314
17315 int32_T exitg1;
17316 b_obj = obj;
17317 cartesian_trajectory_planner_B.b_kstr_c = obj->Base.NameInternal->size[0] *
17318 obj->Base.NameInternal->size[1];
17319 obj->Base.NameInternal->size[0] = 1;
17320 obj->Base.NameInternal->size[1] = 4;
17321 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal,
17322 cartesian_trajectory_planner_B.b_kstr_c);
17323 obj->Base.NameInternal->data[0] = 'b';
17324 obj->Base.NameInternal->data[1] = 'a';
17325 obj->Base.NameInternal->data[2] = 's';
17326 obj->Base.NameInternal->data[3] = 'e';
17327 iobj_15->InTree = false;
17328 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17329 cartesian_trajectory_planner_B.b_kstr_c < 16;
17330 cartesian_trajectory_planner_B.b_kstr_c++) {
17331 iobj_15->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_c] =
17332 tmp[cartesian_trajectory_planner_B.b_kstr_c];
17333 }
17334
17335 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17336 cartesian_trajectory_planner_B.b_kstr_c < 16;
17337 cartesian_trajectory_planner_B.b_kstr_c++) {
17338 iobj_15->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_c] =
17339 tmp[cartesian_trajectory_planner_B.b_kstr_c];
17340 }
17341
17342 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->NameInternal->size[0] *
17343 iobj_15->NameInternal->size[1];
17344 iobj_15->NameInternal->size[0] = 1;
17345 iobj_15->NameInternal->size[1] = 8;
17346 cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal,
17347 cartesian_trajectory_planner_B.b_kstr_c);
17348 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17349 cartesian_trajectory_planner_B.b_kstr_c < 8;
17350 cartesian_trajectory_planner_B.b_kstr_c++) {
17351 iobj_15->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_c] =
17352 tmp_0[cartesian_trajectory_planner_B.b_kstr_c];
17353 }
17354
17355 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->Type->size[0] *
17356 iobj_15->Type->size[1];
17357 iobj_15->Type->size[0] = 1;
17358 iobj_15->Type->size[1] = 5;
17359 cartes_emxEnsureCapacity_char_T(iobj_15->Type,
17360 cartesian_trajectory_planner_B.b_kstr_c);
17361 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17362 cartesian_trajectory_planner_B.b_kstr_c < 5;
17363 cartesian_trajectory_planner_B.b_kstr_c++) {
17364 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c] =
17365 tmp_1[cartesian_trajectory_planner_B.b_kstr_c];
17366 }
17367
17368 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
17369 cartesian_trajectory_planner_B.b_kstr_c = switch_expression->size[0] *
17370 switch_expression->size[1];
17371 switch_expression->size[0] = 1;
17372 switch_expression->size[1] = iobj_15->Type->size[1];
17373 cartes_emxEnsureCapacity_char_T(switch_expression,
17374 cartesian_trajectory_planner_B.b_kstr_c);
17375 cartesian_trajectory_planner_B.loop_ub_a = iobj_15->Type->size[0] *
17376 iobj_15->Type->size[1] - 1;
17377 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17378 cartesian_trajectory_planner_B.b_kstr_c <=
17379 cartesian_trajectory_planner_B.loop_ub_a;
17380 cartesian_trajectory_planner_B.b_kstr_c++) {
17381 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_c] =
17382 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c];
17383 }
17384
17385 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17386 cartesian_trajectory_planner_B.b_kstr_c < 8;
17387 cartesian_trajectory_planner_B.b_kstr_c++) {
17388 cartesian_trajectory_planner_B.b_mx[cartesian_trajectory_planner_B.b_kstr_c]
17389 = tmp_2[cartesian_trajectory_planner_B.b_kstr_c];
17390 }
17391
17392 cartesian_trajectory_planner_B.b_bool_i = false;
17393 if (switch_expression->size[1] == 8) {
17394 cartesian_trajectory_planner_B.b_kstr_c = 1;
17395 do {
17396 exitg1 = 0;
17397 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 8) {
17398 cartesian_trajectory_planner_B.loop_ub_a =
17399 cartesian_trajectory_planner_B.b_kstr_c - 1;
17400 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_a] !=
17401 cartesian_trajectory_planner_B.b_mx[cartesian_trajectory_planner_B.loop_ub_a])
17402 {
17403 exitg1 = 1;
17404 } else {
17405 cartesian_trajectory_planner_B.b_kstr_c++;
17406 }
17407 } else {
17408 cartesian_trajectory_planner_B.b_bool_i = true;
17409 exitg1 = 1;
17410 }
17411 } while (exitg1 == 0);
17412 }
17413
17414 if (cartesian_trajectory_planner_B.b_bool_i) {
17415 cartesian_trajectory_planner_B.b_kstr_c = 0;
17416 } else {
17417 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17418 cartesian_trajectory_planner_B.b_kstr_c < 9;
17419 cartesian_trajectory_planner_B.b_kstr_c++) {
17420 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.b_kstr_c]
17421 = tmp_3[cartesian_trajectory_planner_B.b_kstr_c];
17422 }
17423
17424 cartesian_trajectory_planner_B.b_bool_i = false;
17425 if (switch_expression->size[1] == 9) {
17426 cartesian_trajectory_planner_B.b_kstr_c = 1;
17427 do {
17428 exitg1 = 0;
17429 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 9) {
17430 cartesian_trajectory_planner_B.loop_ub_a =
17431 cartesian_trajectory_planner_B.b_kstr_c - 1;
17432 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_a]
17433 !=
17434 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.loop_ub_a])
17435 {
17436 exitg1 = 1;
17437 } else {
17438 cartesian_trajectory_planner_B.b_kstr_c++;
17439 }
17440 } else {
17441 cartesian_trajectory_planner_B.b_bool_i = true;
17442 exitg1 = 1;
17443 }
17444 } while (exitg1 == 0);
17445 }
17446
17447 if (cartesian_trajectory_planner_B.b_bool_i) {
17448 cartesian_trajectory_planner_B.b_kstr_c = 1;
17449 } else {
17450 cartesian_trajectory_planner_B.b_kstr_c = -1;
17451 }
17452 }
17453
17454 cartesian_trajec_emxFree_char_T(&switch_expression);
17455 switch (cartesian_trajectory_planner_B.b_kstr_c) {
17456 case 0:
17457 cartesian_trajectory_planner_B.iv[0] = 0;
17458 cartesian_trajectory_planner_B.iv[1] = 0;
17459 cartesian_trajectory_planner_B.iv[2] = 1;
17460 cartesian_trajectory_planner_B.iv[3] = 0;
17461 cartesian_trajectory_planner_B.iv[4] = 0;
17462 cartesian_trajectory_planner_B.iv[5] = 0;
17463 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17464 cartesian_trajectory_planner_B.b_kstr_c < 6;
17465 cartesian_trajectory_planner_B.b_kstr_c++) {
17466 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17467 =
17468 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
17469 }
17470
17471 cartesian_trajectory_planner_B.poslim_data[0] = -3.1415926535897931;
17472 cartesian_trajectory_planner_B.poslim_data[1] = 3.1415926535897931;
17473 iobj_15->VelocityNumber = 1.0;
17474 iobj_15->PositionNumber = 1.0;
17475 iobj_15->JointAxisInternal[0] = 0.0;
17476 iobj_15->JointAxisInternal[1] = 0.0;
17477 iobj_15->JointAxisInternal[2] = 1.0;
17478 break;
17479
17480 case 1:
17481 cartesian_trajectory_planner_B.iv[0] = 0;
17482 cartesian_trajectory_planner_B.iv[1] = 0;
17483 cartesian_trajectory_planner_B.iv[2] = 0;
17484 cartesian_trajectory_planner_B.iv[3] = 0;
17485 cartesian_trajectory_planner_B.iv[4] = 0;
17486 cartesian_trajectory_planner_B.iv[5] = 1;
17487 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17488 cartesian_trajectory_planner_B.b_kstr_c < 6;
17489 cartesian_trajectory_planner_B.b_kstr_c++) {
17490 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17491 =
17492 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
17493 }
17494
17495 cartesian_trajectory_planner_B.poslim_data[0] = -0.5;
17496 cartesian_trajectory_planner_B.poslim_data[1] = 0.5;
17497 iobj_15->VelocityNumber = 1.0;
17498 iobj_15->PositionNumber = 1.0;
17499 iobj_15->JointAxisInternal[0] = 0.0;
17500 iobj_15->JointAxisInternal[1] = 0.0;
17501 iobj_15->JointAxisInternal[2] = 1.0;
17502 break;
17503
17504 default:
17505 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17506 cartesian_trajectory_planner_B.b_kstr_c < 6;
17507 cartesian_trajectory_planner_B.b_kstr_c++) {
17508 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17509 = 0;
17510 }
17511
17512 cartesian_trajectory_planner_B.poslim_data[0] = 0.0;
17513 cartesian_trajectory_planner_B.poslim_data[1] = 0.0;
17514 iobj_15->VelocityNumber = 0.0;
17515 iobj_15->PositionNumber = 0.0;
17516 iobj_15->JointAxisInternal[0] = 0.0;
17517 iobj_15->JointAxisInternal[1] = 0.0;
17518 iobj_15->JointAxisInternal[2] = 0.0;
17519 break;
17520 }
17521
17522 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->MotionSubspace->size[0] *
17523 iobj_15->MotionSubspace->size[1];
17524 iobj_15->MotionSubspace->size[0] = 6;
17525 iobj_15->MotionSubspace->size[1] = 1;
17526 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,
17527 cartesian_trajectory_planner_B.b_kstr_c);
17528 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17529 cartesian_trajectory_planner_B.b_kstr_c < 6;
17530 cartesian_trajectory_planner_B.b_kstr_c++) {
17531 iobj_15->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_c] =
17532 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c];
17533 }
17534
17535 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->
17536 PositionLimitsInternal->size[0] * iobj_15->PositionLimitsInternal->size[1];
17537 iobj_15->PositionLimitsInternal->size[0] = 1;
17538 iobj_15->PositionLimitsInternal->size[1] = 2;
17539 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
17540 cartesian_trajectory_planner_B.b_kstr_c);
17541 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17542 cartesian_trajectory_planner_B.b_kstr_c < 2;
17543 cartesian_trajectory_planner_B.b_kstr_c++) {
17544 iobj_15->PositionLimitsInternal->
17545 data[cartesian_trajectory_planner_B.b_kstr_c] =
17546 cartesian_trajectory_planner_B.poslim_data[cartesian_trajectory_planner_B.b_kstr_c];
17547 }
17548
17549 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->HomePositionInternal->size
17550 [0];
17551 iobj_15->HomePositionInternal->size[0] = 1;
17552 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
17553 cartesian_trajectory_planner_B.b_kstr_c);
17554 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17555 cartesian_trajectory_planner_B.b_kstr_c < 1;
17556 cartesian_trajectory_planner_B.b_kstr_c++) {
17557 iobj_15->HomePositionInternal->data[0] = 0.0;
17558 }
17559
17560 obj->Base.JointInternal = iobj_15;
17561 obj->Base.Index = -1.0;
17562 obj->Base.ParentIndex = -1.0;
17563 obj->Base.Index = 0.0;
17564 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr);
17565 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
17566 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
17567 iobj_7, iobj_16);
17568 return b_obj;
17569}
17570
17571static c_rigidBodyJoint_cartesian_as_T *c_rigidBodyJoint_rigidBodyJoint
17572 (c_rigidBodyJoint_cartesian_as_T *obj, const emxArray_char_T_cartesian_tra_T
17573 *jname, const emxArray_char_T_cartesian_tra_T *jtype)
17574{
17575 c_rigidBodyJoint_cartesian_as_T *b_obj;
17576 emxArray_char_T_cartesian_tra_T *switch_expression;
17577 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
17578 };
17579
17580 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17581
17582 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17583
17584 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
17585
17586 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
17587 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
17588 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
17589 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
17590 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
17591 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
17592 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
17593 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
17594 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
17595 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
17596 '{', '|', '}', '~', '\x7f' };
17597
17598 boolean_T guard1 = false;
17599 boolean_T guard2 = false;
17600 boolean_T guard3 = false;
17601 boolean_T guard4 = false;
17602 int32_T exitg1;
17603 boolean_T guard11 = false;
17604 obj->InTree = false;
17605 for (cartesian_trajectory_planner_B.minnanb = 0;
17606 cartesian_trajectory_planner_B.minnanb < 16;
17607 cartesian_trajectory_planner_B.minnanb++) {
17608 obj->JointToParentTransform[cartesian_trajectory_planner_B.minnanb] =
17609 tmp[cartesian_trajectory_planner_B.minnanb];
17610 }
17611
17612 for (cartesian_trajectory_planner_B.minnanb = 0;
17613 cartesian_trajectory_planner_B.minnanb < 16;
17614 cartesian_trajectory_planner_B.minnanb++) {
17615 obj->ChildToJointTransform[cartesian_trajectory_planner_B.minnanb] =
17616 tmp[cartesian_trajectory_planner_B.minnanb];
17617 }
17618
17619 b_obj = obj;
17620 cartesian_trajectory_planner_B.minnanb = obj->NameInternal->size[0] *
17621 obj->NameInternal->size[1];
17622 obj->NameInternal->size[0] = 1;
17623 obj->NameInternal->size[1] = jname->size[1];
17624 cartes_emxEnsureCapacity_char_T(obj->NameInternal,
17625 cartesian_trajectory_planner_B.minnanb);
17626 cartesian_trajectory_planner_B.loop_ub_c = jname->size[0] * jname->size[1] - 1;
17627 for (cartesian_trajectory_planner_B.minnanb = 0;
17628 cartesian_trajectory_planner_B.minnanb <=
17629 cartesian_trajectory_planner_B.loop_ub_c;
17630 cartesian_trajectory_planner_B.minnanb++) {
17631 obj->NameInternal->data[cartesian_trajectory_planner_B.minnanb] =
17632 jname->data[cartesian_trajectory_planner_B.minnanb];
17633 }
17634
17635 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17636 for (cartesian_trajectory_planner_B.minnanb = 0;
17637 cartesian_trajectory_planner_B.minnanb < 8;
17638 cartesian_trajectory_planner_B.minnanb++) {
17639 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb] =
17640 tmp_0[cartesian_trajectory_planner_B.minnanb];
17641 }
17642
17643 guard1 = false;
17644 guard2 = false;
17645 guard3 = false;
17646 guard4 = false;
17647 if (jtype->size[1] <= 8) {
17648 cartesian_trajectory_planner_B.loop_ub_c = jtype->size[1];
17649 for (cartesian_trajectory_planner_B.minnanb = 0;
17650 cartesian_trajectory_planner_B.minnanb < 8;
17651 cartesian_trajectory_planner_B.minnanb++) {
17652 cartesian_trajectory_planner_B.b_g4k[cartesian_trajectory_planner_B.minnanb]
17653 = tmp_0[cartesian_trajectory_planner_B.minnanb];
17654 }
17655
17656 cartesian_trajectory_planner_B.b_bool_ok = false;
17657 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17658 if (cartesian_trajectory_planner_B.minnanb >= 8) {
17659 cartesian_trajectory_planner_B.minnanb = 8;
17660 }
17661
17662 guard11 = false;
17663 if (cartesian_trajectory_planner_B.loop_ub_c <=
17664 cartesian_trajectory_planner_B.minnanb) {
17665 if (cartesian_trajectory_planner_B.minnanb <
17666 cartesian_trajectory_planner_B.loop_ub_c) {
17667 cartesian_trajectory_planner_B.loop_ub_c =
17668 cartesian_trajectory_planner_B.minnanb;
17669 }
17670
17671 cartesian_trajectory_planner_B.minnanb =
17672 cartesian_trajectory_planner_B.loop_ub_c - 1;
17673 guard11 = true;
17674 } else {
17675 if (jtype->size[1] == 8) {
17676 cartesian_trajectory_planner_B.minnanb = 7;
17677 guard11 = true;
17678 }
17679 }
17680
17681 if (guard11) {
17682 cartesian_trajectory_planner_B.loop_ub_c = 1;
17683 do {
17684 exitg1 = 0;
17685 if (cartesian_trajectory_planner_B.loop_ub_c - 1 <=
17686 cartesian_trajectory_planner_B.minnanb) {
17687 cartesian_trajectory_planner_B.kstr_n =
17688 cartesian_trajectory_planner_B.loop_ub_c - 1;
17689 if (tmp_3[static_cast<uint8_T>(jtype->
17690 data[cartesian_trajectory_planner_B.kstr_n]) & 127] != tmp_3[
17691 static_cast<int32_T>
17692 (cartesian_trajectory_planner_B.b_g4k[cartesian_trajectory_planner_B.kstr_n])])
17693 {
17694 exitg1 = 1;
17695 } else {
17696 cartesian_trajectory_planner_B.loop_ub_c++;
17697 }
17698 } else {
17699 cartesian_trajectory_planner_B.b_bool_ok = true;
17700 exitg1 = 1;
17701 }
17702 } while (exitg1 == 0);
17703 }
17704
17705 if (cartesian_trajectory_planner_B.b_bool_ok) {
17706 if (jtype->size[1] == 8) {
17707 cartesian_trajectory_planner_B.nmatched = 1;
17708 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
17709 for (cartesian_trajectory_planner_B.minnanb = 0;
17710 cartesian_trajectory_planner_B.minnanb < 8;
17711 cartesian_trajectory_planner_B.minnanb++) {
17712 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.minnanb]
17713 =
17714 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
17715 }
17716 } else {
17717 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
17718 for (cartesian_trajectory_planner_B.minnanb = 0;
17719 cartesian_trajectory_planner_B.minnanb < 8;
17720 cartesian_trajectory_planner_B.minnanb++) {
17721 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17722 =
17723 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
17724 }
17725
17726 cartesian_trajectory_planner_B.matched = true;
17727 cartesian_trajectory_planner_B.nmatched = 1;
17728 guard3 = true;
17729 }
17730 } else {
17731 guard4 = true;
17732 }
17733 } else {
17734 guard4 = true;
17735 }
17736
17737 if (guard4) {
17738 cartesian_trajectory_planner_B.matched = false;
17739 cartesian_trajectory_planner_B.nmatched = 0;
17740 guard3 = true;
17741 }
17742
17743 if (guard3) {
17744 for (cartesian_trajectory_planner_B.minnanb = 0;
17745 cartesian_trajectory_planner_B.minnanb < 9;
17746 cartesian_trajectory_planner_B.minnanb++) {
17747 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb]
17748 = tmp_1[cartesian_trajectory_planner_B.minnanb];
17749 }
17750
17751 if (jtype->size[1] <= 9) {
17752 cartesian_trajectory_planner_B.loop_ub_c = jtype->size[1];
17753 for (cartesian_trajectory_planner_B.minnanb = 0;
17754 cartesian_trajectory_planner_B.minnanb < 9;
17755 cartesian_trajectory_planner_B.minnanb++) {
17756 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.minnanb]
17757 = tmp_1[cartesian_trajectory_planner_B.minnanb];
17758 }
17759
17760 cartesian_trajectory_planner_B.b_bool_ok = false;
17761 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17762 if (cartesian_trajectory_planner_B.minnanb >= 9) {
17763 cartesian_trajectory_planner_B.minnanb = 9;
17764 }
17765
17766 guard11 = false;
17767 if (cartesian_trajectory_planner_B.loop_ub_c <=
17768 cartesian_trajectory_planner_B.minnanb) {
17769 if (cartesian_trajectory_planner_B.minnanb <
17770 cartesian_trajectory_planner_B.loop_ub_c) {
17771 cartesian_trajectory_planner_B.loop_ub_c =
17772 cartesian_trajectory_planner_B.minnanb;
17773 }
17774
17775 cartesian_trajectory_planner_B.minnanb =
17776 cartesian_trajectory_planner_B.loop_ub_c - 1;
17777 guard11 = true;
17778 } else {
17779 if (jtype->size[1] == 9) {
17780 cartesian_trajectory_planner_B.minnanb = 8;
17781 guard11 = true;
17782 }
17783 }
17784
17785 if (guard11) {
17786 cartesian_trajectory_planner_B.loop_ub_c = 1;
17787 do {
17788 exitg1 = 0;
17789 if (cartesian_trajectory_planner_B.loop_ub_c - 1 <=
17790 cartesian_trajectory_planner_B.minnanb) {
17791 cartesian_trajectory_planner_B.kstr_n =
17792 cartesian_trajectory_planner_B.loop_ub_c - 1;
17793 if (tmp_3[static_cast<uint8_T>(jtype->
17794 data[cartesian_trajectory_planner_B.kstr_n]) & 127] != tmp_3[
17795 static_cast<int32_T>
17796 (cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.kstr_n])])
17797 {
17798 exitg1 = 1;
17799 } else {
17800 cartesian_trajectory_planner_B.loop_ub_c++;
17801 }
17802 } else {
17803 cartesian_trajectory_planner_B.b_bool_ok = true;
17804 exitg1 = 1;
17805 }
17806 } while (exitg1 == 0);
17807 }
17808
17809 if (cartesian_trajectory_planner_B.b_bool_ok) {
17810 if (jtype->size[1] == 9) {
17811 cartesian_trajectory_planner_B.nmatched = 1;
17812 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
17813 for (cartesian_trajectory_planner_B.minnanb = 0;
17814 cartesian_trajectory_planner_B.minnanb < 9;
17815 cartesian_trajectory_planner_B.minnanb++) {
17816 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.minnanb]
17817 =
17818 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
17819 }
17820 } else {
17821 if (!cartesian_trajectory_planner_B.matched) {
17822 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
17823 for (cartesian_trajectory_planner_B.minnanb = 0;
17824 cartesian_trajectory_planner_B.minnanb < 9;
17825 cartesian_trajectory_planner_B.minnanb++) {
17826 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17827 =
17828 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
17829 }
17830 }
17831
17832 cartesian_trajectory_planner_B.matched = true;
17833 cartesian_trajectory_planner_B.nmatched++;
17834 guard2 = true;
17835 }
17836 } else {
17837 guard2 = true;
17838 }
17839 } else {
17840 guard2 = true;
17841 }
17842 }
17843
17844 if (guard2) {
17845 for (cartesian_trajectory_planner_B.minnanb = 0;
17846 cartesian_trajectory_planner_B.minnanb < 5;
17847 cartesian_trajectory_planner_B.minnanb++) {
17848 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb]
17849 = tmp_2[cartesian_trajectory_planner_B.minnanb];
17850 }
17851
17852 if (jtype->size[1] <= 5) {
17853 cartesian_trajectory_planner_B.loop_ub_c = jtype->size[1];
17854 for (cartesian_trajectory_planner_B.minnanb = 0;
17855 cartesian_trajectory_planner_B.minnanb < 5;
17856 cartesian_trajectory_planner_B.minnanb++) {
17857 cartesian_trajectory_planner_B.b_hp[cartesian_trajectory_planner_B.minnanb]
17858 = tmp_2[cartesian_trajectory_planner_B.minnanb];
17859 }
17860
17861 cartesian_trajectory_planner_B.b_bool_ok = false;
17862 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17863 if (cartesian_trajectory_planner_B.minnanb >= 5) {
17864 cartesian_trajectory_planner_B.minnanb = 5;
17865 }
17866
17867 guard11 = false;
17868 if (cartesian_trajectory_planner_B.loop_ub_c <=
17869 cartesian_trajectory_planner_B.minnanb) {
17870 if (cartesian_trajectory_planner_B.minnanb <
17871 cartesian_trajectory_planner_B.loop_ub_c) {
17872 cartesian_trajectory_planner_B.loop_ub_c =
17873 cartesian_trajectory_planner_B.minnanb;
17874 }
17875
17876 cartesian_trajectory_planner_B.minnanb =
17877 cartesian_trajectory_planner_B.loop_ub_c - 1;
17878 guard11 = true;
17879 } else {
17880 if (jtype->size[1] == 5) {
17881 cartesian_trajectory_planner_B.minnanb = 4;
17882 guard11 = true;
17883 }
17884 }
17885
17886 if (guard11) {
17887 cartesian_trajectory_planner_B.loop_ub_c = 1;
17888 do {
17889 exitg1 = 0;
17890 if (cartesian_trajectory_planner_B.loop_ub_c - 1 <=
17891 cartesian_trajectory_planner_B.minnanb) {
17892 cartesian_trajectory_planner_B.kstr_n =
17893 cartesian_trajectory_planner_B.loop_ub_c - 1;
17894 if (tmp_3[static_cast<uint8_T>(jtype->
17895 data[cartesian_trajectory_planner_B.kstr_n]) & 127] != tmp_3[
17896 static_cast<int32_T>
17897 (cartesian_trajectory_planner_B.b_hp[cartesian_trajectory_planner_B.kstr_n])])
17898 {
17899 exitg1 = 1;
17900 } else {
17901 cartesian_trajectory_planner_B.loop_ub_c++;
17902 }
17903 } else {
17904 cartesian_trajectory_planner_B.b_bool_ok = true;
17905 exitg1 = 1;
17906 }
17907 } while (exitg1 == 0);
17908 }
17909
17910 if (cartesian_trajectory_planner_B.b_bool_ok) {
17911 if (jtype->size[1] == 5) {
17912 cartesian_trajectory_planner_B.nmatched = 1;
17913 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
17914 for (cartesian_trajectory_planner_B.minnanb = 0;
17915 cartesian_trajectory_planner_B.minnanb < 5;
17916 cartesian_trajectory_planner_B.minnanb++) {
17917 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.minnanb]
17918 =
17919 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
17920 }
17921 } else {
17922 if (!cartesian_trajectory_planner_B.matched) {
17923 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
17924 for (cartesian_trajectory_planner_B.minnanb = 0;
17925 cartesian_trajectory_planner_B.minnanb < 5;
17926 cartesian_trajectory_planner_B.minnanb++) {
17927 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17928 =
17929 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
17930 }
17931 }
17932
17933 cartesian_trajectory_planner_B.nmatched++;
17934 guard1 = true;
17935 }
17936 } else {
17937 guard1 = true;
17938 }
17939 } else {
17940 guard1 = true;
17941 }
17942 }
17943
17944 if (guard1) {
17945 if (cartesian_trajectory_planner_B.nmatched == 0) {
17946 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17947 } else {
17948 cartesian_trajectory_planner_B.loop_ub_c =
17949 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17950 if (0 <= cartesian_trajectory_planner_B.loop_ub_c) {
17951 memcpy(&cartesian_trajectory_planner_B.b_cj[0],
17952 &cartesian_trajectory_planner_B.partial_match_data[0],
17953 (cartesian_trajectory_planner_B.loop_ub_c + 1) * sizeof(char_T));
17954 }
17955 }
17956 }
17957
17958 if ((cartesian_trajectory_planner_B.nmatched == 0) || ((jtype->size[1] == 0)
17959 != (cartesian_trajectory_planner_B.partial_match_size_idx_1 == 0))) {
17960 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17961 } else {
17962 cartesian_trajectory_planner_B.loop_ub_c =
17963 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17964 if (0 <= cartesian_trajectory_planner_B.loop_ub_c) {
17965 memcpy(&cartesian_trajectory_planner_B.partial_match_data[0],
17966 &cartesian_trajectory_planner_B.b_cj[0],
17967 (cartesian_trajectory_planner_B.loop_ub_c + 1) * sizeof(char_T));
17968 }
17969 }
17970
17971 cartesian_trajectory_planner_B.minnanb = obj->Type->size[0] * obj->Type->size
17972 [1];
17973 obj->Type->size[0] = 1;
17974 obj->Type->size[1] = cartesian_trajectory_planner_B.partial_match_size_idx_1;
17975 cartes_emxEnsureCapacity_char_T(obj->Type,
17976 cartesian_trajectory_planner_B.minnanb);
17977 cartesian_trajectory_planner_B.loop_ub_c =
17978 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17979 for (cartesian_trajectory_planner_B.minnanb = 0;
17980 cartesian_trajectory_planner_B.minnanb <=
17981 cartesian_trajectory_planner_B.loop_ub_c;
17982 cartesian_trajectory_planner_B.minnanb++) {
17983 obj->Type->data[cartesian_trajectory_planner_B.minnanb] =
17984 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb];
17985 }
17986
17987 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
17988 cartesian_trajectory_planner_B.minnanb = switch_expression->size[0] *
17989 switch_expression->size[1];
17990 switch_expression->size[0] = 1;
17991 switch_expression->size[1] = obj->Type->size[1];
17992 cartes_emxEnsureCapacity_char_T(switch_expression,
17993 cartesian_trajectory_planner_B.minnanb);
17994 cartesian_trajectory_planner_B.loop_ub_c = obj->Type->size[0] * obj->
17995 Type->size[1] - 1;
17996 for (cartesian_trajectory_planner_B.minnanb = 0;
17997 cartesian_trajectory_planner_B.minnanb <=
17998 cartesian_trajectory_planner_B.loop_ub_c;
17999 cartesian_trajectory_planner_B.minnanb++) {
18000 switch_expression->data[cartesian_trajectory_planner_B.minnanb] = obj->
18001 Type->data[cartesian_trajectory_planner_B.minnanb];
18002 }
18003
18004 for (cartesian_trajectory_planner_B.minnanb = 0;
18005 cartesian_trajectory_planner_B.minnanb < 8;
18006 cartesian_trajectory_planner_B.minnanb++) {
18007 cartesian_trajectory_planner_B.b_g4k[cartesian_trajectory_planner_B.minnanb]
18008 = tmp_0[cartesian_trajectory_planner_B.minnanb];
18009 }
18010
18011 cartesian_trajectory_planner_B.b_bool_ok = false;
18012 if (switch_expression->size[1] == 8) {
18013 cartesian_trajectory_planner_B.loop_ub_c = 1;
18014 do {
18015 exitg1 = 0;
18016 if (cartesian_trajectory_planner_B.loop_ub_c - 1 < 8) {
18017 cartesian_trajectory_planner_B.kstr_n =
18018 cartesian_trajectory_planner_B.loop_ub_c - 1;
18019 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_n] !=
18020 cartesian_trajectory_planner_B.b_g4k[cartesian_trajectory_planner_B.kstr_n])
18021 {
18022 exitg1 = 1;
18023 } else {
18024 cartesian_trajectory_planner_B.loop_ub_c++;
18025 }
18026 } else {
18027 cartesian_trajectory_planner_B.b_bool_ok = true;
18028 exitg1 = 1;
18029 }
18030 } while (exitg1 == 0);
18031 }
18032
18033 if (cartesian_trajectory_planner_B.b_bool_ok) {
18034 cartesian_trajectory_planner_B.minnanb = 0;
18035 } else {
18036 for (cartesian_trajectory_planner_B.minnanb = 0;
18037 cartesian_trajectory_planner_B.minnanb < 9;
18038 cartesian_trajectory_planner_B.minnanb++) {
18039 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.minnanb]
18040 = tmp_1[cartesian_trajectory_planner_B.minnanb];
18041 }
18042
18043 cartesian_trajectory_planner_B.b_bool_ok = false;
18044 if (switch_expression->size[1] == 9) {
18045 cartesian_trajectory_planner_B.loop_ub_c = 1;
18046 do {
18047 exitg1 = 0;
18048 if (cartesian_trajectory_planner_B.loop_ub_c - 1 < 9) {
18049 cartesian_trajectory_planner_B.kstr_n =
18050 cartesian_trajectory_planner_B.loop_ub_c - 1;
18051 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_n] !=
18052 cartesian_trajectory_planner_B.b_cj[cartesian_trajectory_planner_B.kstr_n])
18053 {
18054 exitg1 = 1;
18055 } else {
18056 cartesian_trajectory_planner_B.loop_ub_c++;
18057 }
18058 } else {
18059 cartesian_trajectory_planner_B.b_bool_ok = true;
18060 exitg1 = 1;
18061 }
18062 } while (exitg1 == 0);
18063 }
18064
18065 if (cartesian_trajectory_planner_B.b_bool_ok) {
18066 cartesian_trajectory_planner_B.minnanb = 1;
18067 } else {
18068 cartesian_trajectory_planner_B.minnanb = -1;
18069 }
18070 }
18071
18072 cartesian_trajec_emxFree_char_T(&switch_expression);
18073 switch (cartesian_trajectory_planner_B.minnanb) {
18074 case 0:
18075 cartesian_trajectory_planner_B.iv3[0] = 0;
18076 cartesian_trajectory_planner_B.iv3[1] = 0;
18077 cartesian_trajectory_planner_B.iv3[2] = 1;
18078 cartesian_trajectory_planner_B.iv3[3] = 0;
18079 cartesian_trajectory_planner_B.iv3[4] = 0;
18080 cartesian_trajectory_planner_B.iv3[5] = 0;
18081 for (cartesian_trajectory_planner_B.minnanb = 0;
18082 cartesian_trajectory_planner_B.minnanb < 6;
18083 cartesian_trajectory_planner_B.minnanb++) {
18084 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.minnanb]
18085 =
18086 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
18087 }
18088
18089 cartesian_trajectory_planner_B.poslim_data_f[0] = -3.1415926535897931;
18090 cartesian_trajectory_planner_B.poslim_data_f[1] = 3.1415926535897931;
18091 obj->VelocityNumber = 1.0;
18092 obj->PositionNumber = 1.0;
18093 obj->JointAxisInternal[0] = 0.0;
18094 obj->JointAxisInternal[1] = 0.0;
18095 obj->JointAxisInternal[2] = 1.0;
18096 break;
18097
18098 case 1:
18099 cartesian_trajectory_planner_B.iv3[0] = 0;
18100 cartesian_trajectory_planner_B.iv3[1] = 0;
18101 cartesian_trajectory_planner_B.iv3[2] = 0;
18102 cartesian_trajectory_planner_B.iv3[3] = 0;
18103 cartesian_trajectory_planner_B.iv3[4] = 0;
18104 cartesian_trajectory_planner_B.iv3[5] = 1;
18105 for (cartesian_trajectory_planner_B.minnanb = 0;
18106 cartesian_trajectory_planner_B.minnanb < 6;
18107 cartesian_trajectory_planner_B.minnanb++) {
18108 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.minnanb]
18109 =
18110 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
18111 }
18112
18113 cartesian_trajectory_planner_B.poslim_data_f[0] = -0.5;
18114 cartesian_trajectory_planner_B.poslim_data_f[1] = 0.5;
18115 obj->VelocityNumber = 1.0;
18116 obj->PositionNumber = 1.0;
18117 obj->JointAxisInternal[0] = 0.0;
18118 obj->JointAxisInternal[1] = 0.0;
18119 obj->JointAxisInternal[2] = 1.0;
18120 break;
18121
18122 default:
18123 for (cartesian_trajectory_planner_B.minnanb = 0;
18124 cartesian_trajectory_planner_B.minnanb < 6;
18125 cartesian_trajectory_planner_B.minnanb++) {
18126 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.minnanb]
18127 = 0;
18128 }
18129
18130 cartesian_trajectory_planner_B.poslim_data_f[0] = 0.0;
18131 cartesian_trajectory_planner_B.poslim_data_f[1] = 0.0;
18132 obj->VelocityNumber = 0.0;
18133 obj->PositionNumber = 0.0;
18134 obj->JointAxisInternal[0] = 0.0;
18135 obj->JointAxisInternal[1] = 0.0;
18136 obj->JointAxisInternal[2] = 0.0;
18137 break;
18138 }
18139
18140 cartesian_trajectory_planner_B.minnanb = obj->MotionSubspace->size[0] *
18141 obj->MotionSubspace->size[1];
18142 obj->MotionSubspace->size[0] = 6;
18143 obj->MotionSubspace->size[1] = 1;
18144 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,
18145 cartesian_trajectory_planner_B.minnanb);
18146 for (cartesian_trajectory_planner_B.minnanb = 0;
18147 cartesian_trajectory_planner_B.minnanb < 6;
18148 cartesian_trajectory_planner_B.minnanb++) {
18149 obj->MotionSubspace->data[cartesian_trajectory_planner_B.minnanb] =
18150 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.minnanb];
18151 }
18152
18153 cartesian_trajectory_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *
18154 obj->PositionLimitsInternal->size[1];
18155 obj->PositionLimitsInternal->size[0] = 1;
18156 obj->PositionLimitsInternal->size[1] = 2;
18157 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,
18158 cartesian_trajectory_planner_B.minnanb);
18159 for (cartesian_trajectory_planner_B.minnanb = 0;
18160 cartesian_trajectory_planner_B.minnanb < 2;
18161 cartesian_trajectory_planner_B.minnanb++) {
18162 obj->PositionLimitsInternal->data[cartesian_trajectory_planner_B.minnanb] =
18163 cartesian_trajectory_planner_B.poslim_data_f[cartesian_trajectory_planner_B.minnanb];
18164 }
18165
18166 cartesian_trajectory_planner_B.minnanb = obj->HomePositionInternal->size[0];
18167 obj->HomePositionInternal->size[0] = 1;
18168 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,
18169 cartesian_trajectory_planner_B.minnanb);
18170 for (cartesian_trajectory_planner_B.minnanb = 0;
18171 cartesian_trajectory_planner_B.minnanb < 1;
18172 cartesian_trajectory_planner_B.minnanb++) {
18173 obj->HomePositionInternal->data[0] = 0.0;
18174 }
18175
18176 return b_obj;
18177}
18178
18179static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
18180 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0,
18181 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
18182 *iobj_2)
18183{
18184 w_robotics_manip_internal_Rig_T *newbody;
18185 c_rigidBodyJoint_cartesian_as_T *newjoint;
18186 emxArray_char_T_cartesian_tra_T *jtype;
18187 emxArray_char_T_cartesian_tra_T *jname;
18188 emxArray_real_T_cartesian_tra_T *obj_0;
18189 emxArray_real_T_cartesian_tra_T *obj_1;
18190 emxArray_real_T_cartesian_tra_T *obj_2;
18191 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
18192 };
18193
18194 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
18195
18196 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18197
18198 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18199
18200 int32_T exitg1;
18201 cartesian_trajec_emxInit_char_T(&jtype, 2);
18202 cartesian_trajectory_planner_B.b_kstr_mu = jtype->size[0] * jtype->size[1];
18203 jtype->size[0] = 1;
18204 jtype->size[1] = obj->NameInternal->size[1];
18205 cartes_emxEnsureCapacity_char_T(jtype,
18206 cartesian_trajectory_planner_B.b_kstr_mu);
18207 cartesian_trajectory_planner_B.loop_ub_gu = obj->NameInternal->size[0] *
18208 obj->NameInternal->size[1] - 1;
18209 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18210 cartesian_trajectory_planner_B.b_kstr_mu <=
18211 cartesian_trajectory_planner_B.loop_ub_gu;
18212 cartesian_trajectory_planner_B.b_kstr_mu++) {
18213 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu] = obj->
18214 NameInternal->data[cartesian_trajectory_planner_B.b_kstr_mu];
18215 }
18216
18217 newbody = iobj_2;
18218 cartesian_trajectory_planner_B.b_kstr_mu = iobj_2->NameInternal->size[0] *
18219 iobj_2->NameInternal->size[1];
18220 iobj_2->NameInternal->size[0] = 1;
18221 iobj_2->NameInternal->size[1] = jtype->size[1];
18222 cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal,
18223 cartesian_trajectory_planner_B.b_kstr_mu);
18224 cartesian_trajectory_planner_B.loop_ub_gu = jtype->size[0] * jtype->size[1] -
18225 1;
18226 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18227 cartesian_trajectory_planner_B.b_kstr_mu <=
18228 cartesian_trajectory_planner_B.loop_ub_gu;
18229 cartesian_trajectory_planner_B.b_kstr_mu++) {
18230 iobj_2->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18231 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu];
18232 }
18233
18234 iobj_0->InTree = false;
18235 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18236 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18237 cartesian_trajectory_planner_B.b_kstr_mu++) {
18238 iobj_0->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_mu] =
18239 tmp[cartesian_trajectory_planner_B.b_kstr_mu];
18240 }
18241
18242 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18243 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18244 cartesian_trajectory_planner_B.b_kstr_mu++) {
18245 iobj_0->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_mu] =
18246 tmp[cartesian_trajectory_planner_B.b_kstr_mu];
18247 }
18248
18249 cartesian_trajectory_planner_B.b_kstr_mu = iobj_0->NameInternal->size[0] *
18250 iobj_0->NameInternal->size[1];
18251 iobj_0->NameInternal->size[0] = 1;
18252 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
18253 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal,
18254 cartesian_trajectory_planner_B.b_kstr_mu);
18255 cartesian_trajectory_planner_B.loop_ub_gu = jtype->size[0] * jtype->size[1];
18256 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18257 cartesian_trajectory_planner_B.b_kstr_mu <
18258 cartesian_trajectory_planner_B.loop_ub_gu;
18259 cartesian_trajectory_planner_B.b_kstr_mu++) {
18260 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18261 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu];
18262 }
18263
18264 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_gu] = '_';
18265 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_gu + 1] =
18266 'j';
18267 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_gu + 2] =
18268 'n';
18269 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_gu + 3] =
18270 't';
18271 cartesian_trajectory_planner_B.b_kstr_mu = iobj_0->Type->size[0] *
18272 iobj_0->Type->size[1];
18273 iobj_0->Type->size[0] = 1;
18274 iobj_0->Type->size[1] = 5;
18275 cartes_emxEnsureCapacity_char_T(iobj_0->Type,
18276 cartesian_trajectory_planner_B.b_kstr_mu);
18277 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18278 cartesian_trajectory_planner_B.b_kstr_mu < 5;
18279 cartesian_trajectory_planner_B.b_kstr_mu++) {
18280 iobj_0->Type->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18281 tmp_0[cartesian_trajectory_planner_B.b_kstr_mu];
18282 }
18283
18284 cartesian_trajectory_planner_B.b_kstr_mu = jtype->size[0] * jtype->size[1];
18285 jtype->size[0] = 1;
18286 jtype->size[1] = iobj_0->Type->size[1];
18287 cartes_emxEnsureCapacity_char_T(jtype,
18288 cartesian_trajectory_planner_B.b_kstr_mu);
18289 cartesian_trajectory_planner_B.loop_ub_gu = iobj_0->Type->size[0] *
18290 iobj_0->Type->size[1] - 1;
18291 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18292 cartesian_trajectory_planner_B.b_kstr_mu <=
18293 cartesian_trajectory_planner_B.loop_ub_gu;
18294 cartesian_trajectory_planner_B.b_kstr_mu++) {
18295 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu] = iobj_0->Type->
18296 data[cartesian_trajectory_planner_B.b_kstr_mu];
18297 }
18298
18299 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18300 cartesian_trajectory_planner_B.b_kstr_mu < 8;
18301 cartesian_trajectory_planner_B.b_kstr_mu++) {
18302 cartesian_trajectory_planner_B.b_dx[cartesian_trajectory_planner_B.b_kstr_mu]
18303 = tmp_1[cartesian_trajectory_planner_B.b_kstr_mu];
18304 }
18305
18306 cartesian_trajectory_planner_B.b_bool_c1 = false;
18307 if (jtype->size[1] == 8) {
18308 cartesian_trajectory_planner_B.b_kstr_mu = 1;
18309 do {
18310 exitg1 = 0;
18311 if (cartesian_trajectory_planner_B.b_kstr_mu - 1 < 8) {
18312 cartesian_trajectory_planner_B.loop_ub_gu =
18313 cartesian_trajectory_planner_B.b_kstr_mu - 1;
18314 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_gu] !=
18315 cartesian_trajectory_planner_B.b_dx[cartesian_trajectory_planner_B.loop_ub_gu])
18316 {
18317 exitg1 = 1;
18318 } else {
18319 cartesian_trajectory_planner_B.b_kstr_mu++;
18320 }
18321 } else {
18322 cartesian_trajectory_planner_B.b_bool_c1 = true;
18323 exitg1 = 1;
18324 }
18325 } while (exitg1 == 0);
18326 }
18327
18328 if (cartesian_trajectory_planner_B.b_bool_c1) {
18329 cartesian_trajectory_planner_B.b_kstr_mu = 0;
18330 } else {
18331 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18332 cartesian_trajectory_planner_B.b_kstr_mu < 9;
18333 cartesian_trajectory_planner_B.b_kstr_mu++) {
18334 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.b_kstr_mu]
18335 = tmp_2[cartesian_trajectory_planner_B.b_kstr_mu];
18336 }
18337
18338 cartesian_trajectory_planner_B.b_bool_c1 = false;
18339 if (jtype->size[1] == 9) {
18340 cartesian_trajectory_planner_B.b_kstr_mu = 1;
18341 do {
18342 exitg1 = 0;
18343 if (cartesian_trajectory_planner_B.b_kstr_mu - 1 < 9) {
18344 cartesian_trajectory_planner_B.loop_ub_gu =
18345 cartesian_trajectory_planner_B.b_kstr_mu - 1;
18346 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_gu] !=
18347 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.loop_ub_gu])
18348 {
18349 exitg1 = 1;
18350 } else {
18351 cartesian_trajectory_planner_B.b_kstr_mu++;
18352 }
18353 } else {
18354 cartesian_trajectory_planner_B.b_bool_c1 = true;
18355 exitg1 = 1;
18356 }
18357 } while (exitg1 == 0);
18358 }
18359
18360 if (cartesian_trajectory_planner_B.b_bool_c1) {
18361 cartesian_trajectory_planner_B.b_kstr_mu = 1;
18362 } else {
18363 cartesian_trajectory_planner_B.b_kstr_mu = -1;
18364 }
18365 }
18366
18367 switch (cartesian_trajectory_planner_B.b_kstr_mu) {
18368 case 0:
18369 cartesian_trajectory_planner_B.iv2[0] = 0;
18370 cartesian_trajectory_planner_B.iv2[1] = 0;
18371 cartesian_trajectory_planner_B.iv2[2] = 1;
18372 cartesian_trajectory_planner_B.iv2[3] = 0;
18373 cartesian_trajectory_planner_B.iv2[4] = 0;
18374 cartesian_trajectory_planner_B.iv2[5] = 0;
18375 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18376 cartesian_trajectory_planner_B.b_kstr_mu < 6;
18377 cartesian_trajectory_planner_B.b_kstr_mu++) {
18378 cartesian_trajectory_planner_B.msubspace_data_a[cartesian_trajectory_planner_B.b_kstr_mu]
18379 =
18380 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_mu];
18381 }
18382
18383 cartesian_trajectory_planner_B.poslim_data_i[0] = -3.1415926535897931;
18384 cartesian_trajectory_planner_B.poslim_data_i[1] = 3.1415926535897931;
18385 iobj_0->VelocityNumber = 1.0;
18386 iobj_0->PositionNumber = 1.0;
18387 iobj_0->JointAxisInternal[0] = 0.0;
18388 iobj_0->JointAxisInternal[1] = 0.0;
18389 iobj_0->JointAxisInternal[2] = 1.0;
18390 break;
18391
18392 case 1:
18393 cartesian_trajectory_planner_B.iv2[0] = 0;
18394 cartesian_trajectory_planner_B.iv2[1] = 0;
18395 cartesian_trajectory_planner_B.iv2[2] = 0;
18396 cartesian_trajectory_planner_B.iv2[3] = 0;
18397 cartesian_trajectory_planner_B.iv2[4] = 0;
18398 cartesian_trajectory_planner_B.iv2[5] = 1;
18399 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18400 cartesian_trajectory_planner_B.b_kstr_mu < 6;
18401 cartesian_trajectory_planner_B.b_kstr_mu++) {
18402 cartesian_trajectory_planner_B.msubspace_data_a[cartesian_trajectory_planner_B.b_kstr_mu]
18403 =
18404 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_mu];
18405 }
18406
18407 cartesian_trajectory_planner_B.poslim_data_i[0] = -0.5;
18408 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.5;
18409 iobj_0->VelocityNumber = 1.0;
18410 iobj_0->PositionNumber = 1.0;
18411 iobj_0->JointAxisInternal[0] = 0.0;
18412 iobj_0->JointAxisInternal[1] = 0.0;
18413 iobj_0->JointAxisInternal[2] = 1.0;
18414 break;
18415
18416 default:
18417 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18418 cartesian_trajectory_planner_B.b_kstr_mu < 6;
18419 cartesian_trajectory_planner_B.b_kstr_mu++) {
18420 cartesian_trajectory_planner_B.msubspace_data_a[cartesian_trajectory_planner_B.b_kstr_mu]
18421 = 0;
18422 }
18423
18424 cartesian_trajectory_planner_B.poslim_data_i[0] = 0.0;
18425 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.0;
18426 iobj_0->VelocityNumber = 0.0;
18427 iobj_0->PositionNumber = 0.0;
18428 iobj_0->JointAxisInternal[0] = 0.0;
18429 iobj_0->JointAxisInternal[1] = 0.0;
18430 iobj_0->JointAxisInternal[2] = 0.0;
18431 break;
18432 }
18433
18434 cartesian_trajectory_planner_B.b_kstr_mu = iobj_0->MotionSubspace->size[0] *
18435 iobj_0->MotionSubspace->size[1];
18436 iobj_0->MotionSubspace->size[0] = 6;
18437 iobj_0->MotionSubspace->size[1] = 1;
18438 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,
18439 cartesian_trajectory_planner_B.b_kstr_mu);
18440 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18441 cartesian_trajectory_planner_B.b_kstr_mu < 6;
18442 cartesian_trajectory_planner_B.b_kstr_mu++) {
18443 iobj_0->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18444 cartesian_trajectory_planner_B.msubspace_data_a[cartesian_trajectory_planner_B.b_kstr_mu];
18445 }
18446
18447 cartesian_trajectory_planner_B.b_kstr_mu = iobj_0->
18448 PositionLimitsInternal->size[0] * iobj_0->PositionLimitsInternal->size[1];
18449 iobj_0->PositionLimitsInternal->size[0] = 1;
18450 iobj_0->PositionLimitsInternal->size[1] = 2;
18451 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
18452 cartesian_trajectory_planner_B.b_kstr_mu);
18453 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18454 cartesian_trajectory_planner_B.b_kstr_mu < 2;
18455 cartesian_trajectory_planner_B.b_kstr_mu++) {
18456 iobj_0->PositionLimitsInternal->
18457 data[cartesian_trajectory_planner_B.b_kstr_mu] =
18458 cartesian_trajectory_planner_B.poslim_data_i[cartesian_trajectory_planner_B.b_kstr_mu];
18459 }
18460
18461 cartesian_trajectory_planner_B.b_kstr_mu = iobj_0->HomePositionInternal->size
18462 [0];
18463 iobj_0->HomePositionInternal->size[0] = 1;
18464 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
18465 cartesian_trajectory_planner_B.b_kstr_mu);
18466 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18467 cartesian_trajectory_planner_B.b_kstr_mu < 1;
18468 cartesian_trajectory_planner_B.b_kstr_mu++) {
18469 iobj_0->HomePositionInternal->data[0] = 0.0;
18470 }
18471
18472 iobj_2->JointInternal = iobj_0;
18473 iobj_2->Index = -1.0;
18474 iobj_2->ParentIndex = -1.0;
18475 cartesian_trajectory_planner_B.b_kstr_mu = jtype->size[0] * jtype->size[1];
18476 jtype->size[0] = 1;
18477 jtype->size[1] = obj->JointInternal.Type->size[1];
18478 cartes_emxEnsureCapacity_char_T(jtype,
18479 cartesian_trajectory_planner_B.b_kstr_mu);
18480 cartesian_trajectory_planner_B.loop_ub_gu = obj->JointInternal.Type->size[0] *
18481 obj->JointInternal.Type->size[1] - 1;
18482 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18483 cartesian_trajectory_planner_B.b_kstr_mu <=
18484 cartesian_trajectory_planner_B.loop_ub_gu;
18485 cartesian_trajectory_planner_B.b_kstr_mu++) {
18486 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18487 obj->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_mu];
18488 }
18489
18490 cartesian_trajec_emxInit_char_T(&jname, 2);
18491 cartesian_trajectory_planner_B.b_kstr_mu = jname->size[0] * jname->size[1];
18492 jname->size[0] = 1;
18493 jname->size[1] = obj->JointInternal.NameInternal->size[1];
18494 cartes_emxEnsureCapacity_char_T(jname,
18495 cartesian_trajectory_planner_B.b_kstr_mu);
18496 cartesian_trajectory_planner_B.loop_ub_gu = obj->
18497 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
18498 [1] - 1;
18499 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18500 cartesian_trajectory_planner_B.b_kstr_mu <=
18501 cartesian_trajectory_planner_B.loop_ub_gu;
18502 cartesian_trajectory_planner_B.b_kstr_mu++) {
18503 jname->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18504 obj->JointInternal.NameInternal->
18505 data[cartesian_trajectory_planner_B.b_kstr_mu];
18506 }
18507
18508 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);
18509 cartesian_trajectory_planner_B.b_kstr_mu = jtype->size[0] * jtype->size[1];
18510 jtype->size[0] = 1;
18511 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
18512 cartes_emxEnsureCapacity_char_T(jtype,
18513 cartesian_trajectory_planner_B.b_kstr_mu);
18514 cartesian_trajectory_planner_B.loop_ub_gu = obj->
18515 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
18516 [1] - 1;
18517 cartesian_trajec_emxFree_char_T(&jname);
18518 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18519 cartesian_trajectory_planner_B.b_kstr_mu <=
18520 cartesian_trajectory_planner_B.loop_ub_gu;
18521 cartesian_trajectory_planner_B.b_kstr_mu++) {
18522 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18523 obj->JointInternal.NameInternal->
18524 data[cartesian_trajectory_planner_B.b_kstr_mu];
18525 }
18526
18527 if (jtype->size[1] != 0) {
18528 cartesian_trajectory_planner_B.b_kstr_mu = jtype->size[0] * jtype->size[1];
18529 jtype->size[0] = 1;
18530 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
18531 cartes_emxEnsureCapacity_char_T(jtype,
18532 cartesian_trajectory_planner_B.b_kstr_mu);
18533 cartesian_trajectory_planner_B.loop_ub_gu = obj->
18534 JointInternal.NameInternal->size[0] * obj->
18535 JointInternal.NameInternal->size[1] - 1;
18536 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18537 cartesian_trajectory_planner_B.b_kstr_mu <=
18538 cartesian_trajectory_planner_B.loop_ub_gu;
18539 cartesian_trajectory_planner_B.b_kstr_mu++) {
18540 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18541 obj->JointInternal.NameInternal->
18542 data[cartesian_trajectory_planner_B.b_kstr_mu];
18543 }
18544
18545 if (!newjoint->InTree) {
18546 cartesian_trajectory_planner_B.b_kstr_mu = newjoint->NameInternal->size[0]
18547 * newjoint->NameInternal->size[1];
18548 newjoint->NameInternal->size[0] = 1;
18549 newjoint->NameInternal->size[1] = jtype->size[1];
18550 cartes_emxEnsureCapacity_char_T(newjoint->NameInternal,
18551 cartesian_trajectory_planner_B.b_kstr_mu);
18552 cartesian_trajectory_planner_B.loop_ub_gu = jtype->size[0] * jtype->size[1]
18553 - 1;
18554 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18555 cartesian_trajectory_planner_B.b_kstr_mu <=
18556 cartesian_trajectory_planner_B.loop_ub_gu;
18557 cartesian_trajectory_planner_B.b_kstr_mu++) {
18558 newjoint->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18559 jtype->data[cartesian_trajectory_planner_B.b_kstr_mu];
18560 }
18561 }
18562 }
18563
18564 cartesian_trajec_emxFree_char_T(&jtype);
18565 cartesian_trajec_emxInit_real_T(&obj_0, 1);
18566 cartesian_trajectory_planner_B.loop_ub_gu =
18567 obj->JointInternal.PositionLimitsInternal->size[0] *
18568 obj->JointInternal.PositionLimitsInternal->size[1];
18569 cartesian_trajectory_planner_B.b_kstr_mu = newjoint->
18570 PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];
18571 newjoint->PositionLimitsInternal->size[0] =
18572 obj->JointInternal.PositionLimitsInternal->size[0];
18573 newjoint->PositionLimitsInternal->size[1] = 2;
18574 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
18575 cartesian_trajectory_planner_B.b_kstr_mu);
18576 cartesian_trajectory_planner_B.b_kstr_mu = obj_0->size[0];
18577 obj_0->size[0] = cartesian_trajectory_planner_B.loop_ub_gu;
18578 cartes_emxEnsureCapacity_real_T(obj_0,
18579 cartesian_trajectory_planner_B.b_kstr_mu);
18580 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18581 cartesian_trajectory_planner_B.b_kstr_mu <
18582 cartesian_trajectory_planner_B.loop_ub_gu;
18583 cartesian_trajectory_planner_B.b_kstr_mu++) {
18584 obj_0->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18585 obj->JointInternal.PositionLimitsInternal->
18586 data[cartesian_trajectory_planner_B.b_kstr_mu];
18587 }
18588
18589 cartesian_trajectory_planner_B.loop_ub_gu = obj_0->size[0];
18590 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18591 cartesian_trajectory_planner_B.b_kstr_mu <
18592 cartesian_trajectory_planner_B.loop_ub_gu;
18593 cartesian_trajectory_planner_B.b_kstr_mu++) {
18594 newjoint->PositionLimitsInternal->
18595 data[cartesian_trajectory_planner_B.b_kstr_mu] = obj_0->
18596 data[cartesian_trajectory_planner_B.b_kstr_mu];
18597 }
18598
18599 cartesian_trajec_emxFree_real_T(&obj_0);
18600 cartesian_trajec_emxInit_real_T(&obj_1, 1);
18601 cartesian_trajectory_planner_B.b_kstr_mu = obj_1->size[0];
18602 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
18603 cartes_emxEnsureCapacity_real_T(obj_1,
18604 cartesian_trajectory_planner_B.b_kstr_mu);
18605 cartesian_trajectory_planner_B.loop_ub_gu =
18606 obj->JointInternal.HomePositionInternal->size[0];
18607 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18608 cartesian_trajectory_planner_B.b_kstr_mu <
18609 cartesian_trajectory_planner_B.loop_ub_gu;
18610 cartesian_trajectory_planner_B.b_kstr_mu++) {
18611 obj_1->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18612 obj->JointInternal.HomePositionInternal->
18613 data[cartesian_trajectory_planner_B.b_kstr_mu];
18614 }
18615
18616 cartesian_trajectory_planner_B.b_kstr_mu = newjoint->
18617 HomePositionInternal->size[0];
18618 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
18619 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
18620 cartesian_trajectory_planner_B.b_kstr_mu);
18621 cartesian_trajectory_planner_B.loop_ub_gu = obj_1->size[0];
18622 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18623 cartesian_trajectory_planner_B.b_kstr_mu <
18624 cartesian_trajectory_planner_B.loop_ub_gu;
18625 cartesian_trajectory_planner_B.b_kstr_mu++) {
18626 newjoint->HomePositionInternal->
18627 data[cartesian_trajectory_planner_B.b_kstr_mu] = obj_1->
18628 data[cartesian_trajectory_planner_B.b_kstr_mu];
18629 }
18630
18631 cartesian_trajec_emxFree_real_T(&obj_1);
18632 cartesian_trajectory_planner_B.obj_idx_0 =
18633 obj->JointInternal.JointAxisInternal[0];
18634 cartesian_trajectory_planner_B.obj_idx_1 =
18635 obj->JointInternal.JointAxisInternal[1];
18636 cartesian_trajectory_planner_B.obj_idx_2 =
18637 obj->JointInternal.JointAxisInternal[2];
18638 newjoint->JointAxisInternal[0] = cartesian_trajectory_planner_B.obj_idx_0;
18639 newjoint->JointAxisInternal[1] = cartesian_trajectory_planner_B.obj_idx_1;
18640 newjoint->JointAxisInternal[2] = cartesian_trajectory_planner_B.obj_idx_2;
18641 cartesian_trajec_emxInit_real_T(&obj_2, 1);
18642 cartesian_trajectory_planner_B.loop_ub_gu = obj->
18643 JointInternal.MotionSubspace->size[0] * obj->
18644 JointInternal.MotionSubspace->size[1];
18645 cartesian_trajectory_planner_B.b_kstr_mu = newjoint->MotionSubspace->size[0] *
18646 newjoint->MotionSubspace->size[1];
18647 newjoint->MotionSubspace->size[0] = 6;
18648 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
18649 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,
18650 cartesian_trajectory_planner_B.b_kstr_mu);
18651 cartesian_trajectory_planner_B.b_kstr_mu = obj_2->size[0];
18652 obj_2->size[0] = cartesian_trajectory_planner_B.loop_ub_gu;
18653 cartes_emxEnsureCapacity_real_T(obj_2,
18654 cartesian_trajectory_planner_B.b_kstr_mu);
18655 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18656 cartesian_trajectory_planner_B.b_kstr_mu <
18657 cartesian_trajectory_planner_B.loop_ub_gu;
18658 cartesian_trajectory_planner_B.b_kstr_mu++) {
18659 obj_2->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18660 obj->JointInternal.MotionSubspace->
18661 data[cartesian_trajectory_planner_B.b_kstr_mu];
18662 }
18663
18664 cartesian_trajectory_planner_B.loop_ub_gu = obj_2->size[0];
18665 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18666 cartesian_trajectory_planner_B.b_kstr_mu <
18667 cartesian_trajectory_planner_B.loop_ub_gu;
18668 cartesian_trajectory_planner_B.b_kstr_mu++) {
18669 newjoint->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_mu] =
18670 obj_2->data[cartesian_trajectory_planner_B.b_kstr_mu];
18671 }
18672
18673 cartesian_trajec_emxFree_real_T(&obj_2);
18674 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18675 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18676 cartesian_trajectory_planner_B.b_kstr_mu++) {
18677 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_mu]
18678 = obj->
18679 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_mu];
18680 }
18681
18682 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18683 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18684 cartesian_trajectory_planner_B.b_kstr_mu++) {
18685 newjoint->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_mu] =
18686 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_mu];
18687 }
18688
18689 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18690 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18691 cartesian_trajectory_planner_B.b_kstr_mu++) {
18692 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_mu]
18693 = obj->
18694 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_mu];
18695 }
18696
18697 for (cartesian_trajectory_planner_B.b_kstr_mu = 0;
18698 cartesian_trajectory_planner_B.b_kstr_mu < 16;
18699 cartesian_trajectory_planner_B.b_kstr_mu++) {
18700 newjoint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_mu] =
18701 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_mu];
18702 }
18703
18704 iobj_2->JointInternal = newjoint;
18705 return newbody;
18706}
18707
18708static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
18709 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
18710 *parentName, c_rigidBodyJoint_cartesian_as_T *iobj_0,
18711 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
18712 *iobj_2)
18713{
18714 w_robotics_manip_internal_Rig_T *body;
18715 c_rigidBodyJoint_cartesian_as_T *jnt;
18716 emxArray_char_T_cartesian_tra_T *bname;
18717 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
18718
18719 boolean_T exitg1;
18720 int32_T exitg2;
18721 cartesian_trajec_emxInit_char_T(&bname, 2);
18722 cartesian_trajectory_planner_B.pid = -1.0;
18723 cartesian_trajectory_planner_B.b_kstr_f = bname->size[0] * bname->size[1];
18724 bname->size[0] = 1;
18725 bname->size[1] = obj->Base.NameInternal->size[1];
18726 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr_f);
18727 cartesian_trajectory_planner_B.loop_ub_ay = obj->Base.NameInternal->size[0] *
18728 obj->Base.NameInternal->size[1] - 1;
18729 for (cartesian_trajectory_planner_B.b_kstr_f = 0;
18730 cartesian_trajectory_planner_B.b_kstr_f <=
18731 cartesian_trajectory_planner_B.loop_ub_ay;
18732 cartesian_trajectory_planner_B.b_kstr_f++) {
18733 bname->data[cartesian_trajectory_planner_B.b_kstr_f] =
18734 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_f];
18735 }
18736
18737 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
18738 cartesian_trajectory_planner_B.pid = 0.0;
18739 } else {
18740 cartesian_trajectory_planner_B.b_index_f = obj->NumBodies;
18741 cartesian_trajectory_planner_B.b_i_n = 0;
18742 exitg1 = false;
18743 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_n <= static_cast<
18744 int32_T>(cartesian_trajectory_planner_B.b_index_f) - 1))
18745 {
18746 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_n];
18747 cartesian_trajectory_planner_B.b_kstr_f = bname->size[0] * bname->size[1];
18748 bname->size[0] = 1;
18749 bname->size[1] = body->NameInternal->size[1];
18750 cartes_emxEnsureCapacity_char_T(bname,
18751 cartesian_trajectory_planner_B.b_kstr_f);
18752 cartesian_trajectory_planner_B.loop_ub_ay = body->NameInternal->size[0] *
18753 body->NameInternal->size[1] - 1;
18754 for (cartesian_trajectory_planner_B.b_kstr_f = 0;
18755 cartesian_trajectory_planner_B.b_kstr_f <=
18756 cartesian_trajectory_planner_B.loop_ub_ay;
18757 cartesian_trajectory_planner_B.b_kstr_f++) {
18758 bname->data[cartesian_trajectory_planner_B.b_kstr_f] =
18759 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_f];
18760 }
18761
18762 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
18763 cartesian_trajectory_planner_B.pid = static_cast<real_T>
18764 (cartesian_trajectory_planner_B.b_i_n) + 1.0;
18765 exitg1 = true;
18766 } else {
18767 cartesian_trajectory_planner_B.b_i_n++;
18768 }
18769 }
18770 }
18771
18772 cartesian_trajectory_planner_B.b_index_f = obj->NumBodies + 1.0;
18773 body = cartesian_trajec_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);
18774 obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index_f) - 1]
18775 = body;
18776 body->Index = cartesian_trajectory_planner_B.b_index_f;
18777 body->ParentIndex = cartesian_trajectory_planner_B.pid;
18778 body->JointInternal->InTree = true;
18779 obj->NumBodies++;
18780 jnt = body->JointInternal;
18781 cartesian_trajectory_planner_B.b_kstr_f = bname->size[0] * bname->size[1];
18782 bname->size[0] = 1;
18783 bname->size[1] = jnt->Type->size[1];
18784 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr_f);
18785 cartesian_trajectory_planner_B.loop_ub_ay = jnt->Type->size[0] * jnt->
18786 Type->size[1] - 1;
18787 for (cartesian_trajectory_planner_B.b_kstr_f = 0;
18788 cartesian_trajectory_planner_B.b_kstr_f <=
18789 cartesian_trajectory_planner_B.loop_ub_ay;
18790 cartesian_trajectory_planner_B.b_kstr_f++) {
18791 bname->data[cartesian_trajectory_planner_B.b_kstr_f] = jnt->Type->
18792 data[cartesian_trajectory_planner_B.b_kstr_f];
18793 }
18794
18795 for (cartesian_trajectory_planner_B.b_kstr_f = 0;
18796 cartesian_trajectory_planner_B.b_kstr_f < 5;
18797 cartesian_trajectory_planner_B.b_kstr_f++) {
18798 cartesian_trajectory_planner_B.b_ho[cartesian_trajectory_planner_B.b_kstr_f]
18799 = tmp[cartesian_trajectory_planner_B.b_kstr_f];
18800 }
18801
18802 cartesian_trajectory_planner_B.b_bool_g = false;
18803 if (bname->size[1] == 5) {
18804 cartesian_trajectory_planner_B.b_kstr_f = 1;
18805 do {
18806 exitg2 = 0;
18807 if (cartesian_trajectory_planner_B.b_kstr_f - 1 < 5) {
18808 cartesian_trajectory_planner_B.loop_ub_ay =
18809 cartesian_trajectory_planner_B.b_kstr_f - 1;
18810 if (bname->data[cartesian_trajectory_planner_B.loop_ub_ay] !=
18811 cartesian_trajectory_planner_B.b_ho[cartesian_trajectory_planner_B.loop_ub_ay])
18812 {
18813 exitg2 = 1;
18814 } else {
18815 cartesian_trajectory_planner_B.b_kstr_f++;
18816 }
18817 } else {
18818 cartesian_trajectory_planner_B.b_bool_g = true;
18819 exitg2 = 1;
18820 }
18821 } while (exitg2 == 0);
18822 }
18823
18824 cartesian_trajec_emxFree_char_T(&bname);
18825 if (!cartesian_trajectory_planner_B.b_bool_g) {
18826 obj->NumNonFixedBodies++;
18827 jnt = body->JointInternal;
18828 cartesian_trajectory_planner_B.b_kstr_f = static_cast<int32_T>(body->Index)
18829 - 1;
18830 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_f] =
18831 obj->PositionNumber + 1.0;
18832 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_f + 8] =
18833 obj->PositionNumber + jnt->PositionNumber;
18834 jnt = body->JointInternal;
18835 cartesian_trajectory_planner_B.b_kstr_f = static_cast<int32_T>(body->Index)
18836 - 1;
18837 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_f] =
18838 obj->VelocityNumber + 1.0;
18839 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_f + 8] =
18840 obj->VelocityNumber + jnt->VelocityNumber;
18841 } else {
18842 cartesian_trajectory_planner_B.b_kstr_f = static_cast<int32_T>(body->Index);
18843 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_f - 1] = 0.0;
18844 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_f + 7] = -1.0;
18845 cartesian_trajectory_planner_B.b_kstr_f = static_cast<int32_T>(body->Index);
18846 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_f - 1] = 0.0;
18847 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_f + 7] = -1.0;
18848 }
18849
18850 jnt = body->JointInternal;
18851 obj->PositionNumber += jnt->PositionNumber;
18852 jnt = body->JointInternal;
18853 obj->VelocityNumber += jnt->VelocityNumber;
18854}
18855
18856static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
18857 y_robotics_manip_internal_Rig_T *rigidbodytree,
18858 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
18859 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
18860 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
18861 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
18862 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
18863 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
18864 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
18865 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
18866 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
18867 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
18868 c_rigidBodyJoint_cartesian_as_T *iobj_15, c_rigidBodyJoint_cartesian_as_T
18869 *iobj_16, c_rigidBodyJoint_cartesian_as_T *iobj_17,
18870 c_rigidBodyJoint_cartesian_as_T *iobj_18, c_rigidBodyJoint_cartesian_as_T
18871 *iobj_19, c_rigidBodyJoint_cartesian_as_T *iobj_20,
18872 c_rigidBodyJoint_cartesian_as_T *iobj_21, c_rigidBodyJoint_cartesian_as_T
18873 *iobj_22, c_rigidBodyJoint_cartesian_as_T *iobj_23,
18874 c_rigidBodyJoint_cartesian_as_T *iobj_24, c_rigidBodyJoint_cartesian_as_T
18875 *iobj_25, c_rigidBodyJoint_cartesian_as_T *iobj_26,
18876 c_rigidBodyJoint_cartesian_as_T *iobj_27, c_rigidBodyJoint_cartesian_as_T
18877 *iobj_28, c_rigidBodyJoint_cartesian_as_T *iobj_29,
18878 c_rigidBodyJoint_cartesian_as_T *iobj_30, c_rigidBodyJoint_cartesian_as_T
18879 *iobj_31, c_rigidBodyJoint_cartesian_as_T *iobj_32,
18880 c_rigidBodyJoint_cartesian_as_T *iobj_33, c_rigidBodyJoint_cartesian_as_T
18881 *iobj_34, c_rigidBodyJoint_cartesian_as_T *iobj_35,
18882 c_rigidBodyJoint_cartesian_as_T *iobj_36, c_rigidBodyJoint_cartesian_as_T
18883 *iobj_37, c_rigidBodyJoint_cartesian_as_T *iobj_38,
18884 c_rigidBodyJoint_cartesian_as_T *iobj_39, w_robotics_manip_internal_Rig_T
18885 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)
18886{
18887 x_robotics_manip_internal_Rig_T *newrobot;
18888 v_robotics_manip_internal_Rig_T *body;
18889 v_robotics_manip_internal_Rig_T *parent;
18890 emxArray_char_T_cartesian_tra_T *b_basename;
18891 w_robotics_manip_internal_Rig_T *body_0;
18892 c_rigidBodyJoint_cartesian_as_T *jnt;
18893 emxArray_char_T_cartesian_tra_T *bname;
18894 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
18895
18896 boolean_T exitg1;
18897 int32_T exitg2;
18898 cartesian_trajec_emxInit_char_T(&b_basename, 2);
18899 newrobot = RigidBodyTree_RigidBodyTree_ast(iobj_41, iobj_0, iobj_1, iobj_2,
18900 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
18901 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);
18902 cartesian_trajectory_planner_B.b_kstr_o = b_basename->size[0] *
18903 b_basename->size[1];
18904 b_basename->size[0] = 1;
18905 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
18906 cartes_emxEnsureCapacity_char_T(b_basename,
18907 cartesian_trajectory_planner_B.b_kstr_o);
18908 cartesian_trajectory_planner_B.loop_ub_hm = rigidbodytree->
18909 Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;
18910 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
18911 cartesian_trajectory_planner_B.b_kstr_o <=
18912 cartesian_trajectory_planner_B.loop_ub_hm;
18913 cartesian_trajectory_planner_B.b_kstr_o++) {
18914 b_basename->data[cartesian_trajectory_planner_B.b_kstr_o] =
18915 rigidbodytree->Base.NameInternal->
18916 data[cartesian_trajectory_planner_B.b_kstr_o];
18917 }
18918
18919 cartesian_trajec_emxInit_char_T(&bname, 2);
18920 cartesian_trajectory_planner_B.bid_a = -1.0;
18921 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
18922 bname->size[0] = 1;
18923 bname->size[1] = newrobot->Base.NameInternal->size[1];
18924 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr_o);
18925 cartesian_trajectory_planner_B.loop_ub_hm = newrobot->Base.NameInternal->size
18926 [0] * newrobot->Base.NameInternal->size[1] - 1;
18927 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
18928 cartesian_trajectory_planner_B.b_kstr_o <=
18929 cartesian_trajectory_planner_B.loop_ub_hm;
18930 cartesian_trajectory_planner_B.b_kstr_o++) {
18931 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
18932 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
18933 }
18934
18935 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
18936 cartesian_trajectory_planner_B.bid_a = 0.0;
18937 } else {
18938 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
18939 cartesian_trajectory_planner_B.b_i_g = 0;
18940 exitg1 = false;
18941 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_g <=
18942 static_cast<int32_T>
18943 (cartesian_trajectory_planner_B.b_index) - 1)) {
18944 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_g];
18945 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
18946 bname->size[0] = 1;
18947 bname->size[1] = body_0->NameInternal->size[1];
18948 cartes_emxEnsureCapacity_char_T(bname,
18949 cartesian_trajectory_planner_B.b_kstr_o);
18950 cartesian_trajectory_planner_B.loop_ub_hm = body_0->NameInternal->size[0] *
18951 body_0->NameInternal->size[1] - 1;
18952 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
18953 cartesian_trajectory_planner_B.b_kstr_o <=
18954 cartesian_trajectory_planner_B.loop_ub_hm;
18955 cartesian_trajectory_planner_B.b_kstr_o++) {
18956 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
18957 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
18958 }
18959
18960 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
18961 cartesian_trajectory_planner_B.bid_a = static_cast<real_T>
18962 (cartesian_trajectory_planner_B.b_i_g) + 1.0;
18963 exitg1 = true;
18964 } else {
18965 cartesian_trajectory_planner_B.b_i_g++;
18966 }
18967 }
18968 }
18969
18970 if ((!(cartesian_trajectory_planner_B.bid_a == 0.0)) &&
18971 (cartesian_trajectory_planner_B.bid_a < 0.0)) {
18972 cartesian_trajectory_planner_B.b_kstr_o = newrobot->Base.NameInternal->size
18973 [0] * newrobot->Base.NameInternal->size[1];
18974 newrobot->Base.NameInternal->size[0] = 1;
18975 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
18976 cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,
18977 cartesian_trajectory_planner_B.b_kstr_o);
18978 cartesian_trajectory_planner_B.loop_ub_hm = b_basename->size[0] *
18979 b_basename->size[1] - 1;
18980 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
18981 cartesian_trajectory_planner_B.b_kstr_o <=
18982 cartesian_trajectory_planner_B.loop_ub_hm;
18983 cartesian_trajectory_planner_B.b_kstr_o++) {
18984 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o]
18985 = b_basename->data[cartesian_trajectory_planner_B.b_kstr_o];
18986 }
18987 }
18988
18989 if (1.0 <= rigidbodytree->NumBodies) {
18990 body = rigidbodytree->Bodies[0];
18991 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
18992 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
18993 parent = rigidbodytree->Bodies[static_cast<int32_T>
18994 (cartesian_trajectory_planner_B.bid_a) - 1];
18995 } else {
18996 parent = &rigidbodytree->Base;
18997 }
18998
18999 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19000 bname->size[0] = 1;
19001 bname->size[1] = parent->NameInternal->size[1];
19002 cartes_emxEnsureCapacity_char_T(bname,
19003 cartesian_trajectory_planner_B.b_kstr_o);
19004 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19005 parent->NameInternal->size[1] - 1;
19006 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19007 cartesian_trajectory_planner_B.b_kstr_o <=
19008 cartesian_trajectory_planner_B.loop_ub_hm;
19009 cartesian_trajectory_planner_B.b_kstr_o++) {
19010 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19011 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19012 }
19013
19014 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
19015 iobj_7);
19016 }
19017
19018 if (2.0 <= rigidbodytree->NumBodies) {
19019 body = rigidbodytree->Bodies[1];
19020 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19021 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19022 parent = rigidbodytree->Bodies[static_cast<int32_T>
19023 (cartesian_trajectory_planner_B.bid_a) - 1];
19024 } else {
19025 parent = &rigidbodytree->Base;
19026 }
19027
19028 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19029 bname->size[0] = 1;
19030 bname->size[1] = parent->NameInternal->size[1];
19031 cartes_emxEnsureCapacity_char_T(bname,
19032 cartesian_trajectory_planner_B.b_kstr_o);
19033 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19034 parent->NameInternal->size[1] - 1;
19035 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19036 cartesian_trajectory_planner_B.b_kstr_o <=
19037 cartesian_trajectory_planner_B.loop_ub_hm;
19038 cartesian_trajectory_planner_B.b_kstr_o++) {
19039 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19040 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19041 }
19042
19043 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
19044 iobj_8);
19045 }
19046
19047 if (3.0 <= rigidbodytree->NumBodies) {
19048 body = rigidbodytree->Bodies[2];
19049 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19050 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19051 parent = rigidbodytree->Bodies[static_cast<int32_T>
19052 (cartesian_trajectory_planner_B.bid_a) - 1];
19053 } else {
19054 parent = &rigidbodytree->Base;
19055 }
19056
19057 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19058 bname->size[0] = 1;
19059 bname->size[1] = parent->NameInternal->size[1];
19060 cartes_emxEnsureCapacity_char_T(bname,
19061 cartesian_trajectory_planner_B.b_kstr_o);
19062 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19063 parent->NameInternal->size[1] - 1;
19064 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19065 cartesian_trajectory_planner_B.b_kstr_o <=
19066 cartesian_trajectory_planner_B.loop_ub_hm;
19067 cartesian_trajectory_planner_B.b_kstr_o++) {
19068 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19069 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19070 }
19071
19072 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
19073 iobj_9);
19074 }
19075
19076 if (4.0 <= rigidbodytree->NumBodies) {
19077 body = rigidbodytree->Bodies[3];
19078 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19079 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19080 parent = rigidbodytree->Bodies[static_cast<int32_T>
19081 (cartesian_trajectory_planner_B.bid_a) - 1];
19082 } else {
19083 parent = &rigidbodytree->Base;
19084 }
19085
19086 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19087 bname->size[0] = 1;
19088 bname->size[1] = parent->NameInternal->size[1];
19089 cartes_emxEnsureCapacity_char_T(bname,
19090 cartesian_trajectory_planner_B.b_kstr_o);
19091 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19092 parent->NameInternal->size[1] - 1;
19093 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19094 cartesian_trajectory_planner_B.b_kstr_o <=
19095 cartesian_trajectory_planner_B.loop_ub_hm;
19096 cartesian_trajectory_planner_B.b_kstr_o++) {
19097 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19098 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19099 }
19100
19101 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
19102 iobj_10);
19103 }
19104
19105 if (5.0 <= rigidbodytree->NumBodies) {
19106 body = rigidbodytree->Bodies[4];
19107 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19108 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19109 parent = rigidbodytree->Bodies[static_cast<int32_T>
19110 (cartesian_trajectory_planner_B.bid_a) - 1];
19111 } else {
19112 parent = &rigidbodytree->Base;
19113 }
19114
19115 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19116 bname->size[0] = 1;
19117 bname->size[1] = parent->NameInternal->size[1];
19118 cartes_emxEnsureCapacity_char_T(bname,
19119 cartesian_trajectory_planner_B.b_kstr_o);
19120 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19121 parent->NameInternal->size[1] - 1;
19122 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19123 cartesian_trajectory_planner_B.b_kstr_o <=
19124 cartesian_trajectory_planner_B.loop_ub_hm;
19125 cartesian_trajectory_planner_B.b_kstr_o++) {
19126 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19127 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19128 }
19129
19130 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
19131 iobj_11);
19132 }
19133
19134 if (6.0 <= rigidbodytree->NumBodies) {
19135 body = rigidbodytree->Bodies[5];
19136 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19137 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19138 parent = rigidbodytree->Bodies[static_cast<int32_T>
19139 (cartesian_trajectory_planner_B.bid_a) - 1];
19140 } else {
19141 parent = &rigidbodytree->Base;
19142 }
19143
19144 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19145 bname->size[0] = 1;
19146 bname->size[1] = parent->NameInternal->size[1];
19147 cartes_emxEnsureCapacity_char_T(bname,
19148 cartesian_trajectory_planner_B.b_kstr_o);
19149 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19150 parent->NameInternal->size[1] - 1;
19151 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19152 cartesian_trajectory_planner_B.b_kstr_o <=
19153 cartesian_trajectory_planner_B.loop_ub_hm;
19154 cartesian_trajectory_planner_B.b_kstr_o++) {
19155 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19156 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19157 }
19158
19159 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
19160 iobj_12);
19161 }
19162
19163 if (7.0 <= rigidbodytree->NumBodies) {
19164 body = rigidbodytree->Bodies[6];
19165 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19166 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19167 parent = rigidbodytree->Bodies[static_cast<int32_T>
19168 (cartesian_trajectory_planner_B.bid_a) - 1];
19169 } else {
19170 parent = &rigidbodytree->Base;
19171 }
19172
19173 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19174 bname->size[0] = 1;
19175 bname->size[1] = parent->NameInternal->size[1];
19176 cartes_emxEnsureCapacity_char_T(bname,
19177 cartesian_trajectory_planner_B.b_kstr_o);
19178 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19179 parent->NameInternal->size[1] - 1;
19180 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19181 cartesian_trajectory_planner_B.b_kstr_o <=
19182 cartesian_trajectory_planner_B.loop_ub_hm;
19183 cartesian_trajectory_planner_B.b_kstr_o++) {
19184 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19185 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19186 }
19187
19188 cartesian_trajectory_planner_B.bid_a = -1.0;
19189 cartesian_trajectory_planner_B.b_kstr_o = b_basename->size[0] *
19190 b_basename->size[1];
19191 b_basename->size[0] = 1;
19192 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
19193 cartes_emxEnsureCapacity_char_T(b_basename,
19194 cartesian_trajectory_planner_B.b_kstr_o);
19195 cartesian_trajectory_planner_B.loop_ub_hm = newrobot->
19196 Base.NameInternal->size[0] * newrobot->Base.NameInternal->size[1] - 1;
19197 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19198 cartesian_trajectory_planner_B.b_kstr_o <=
19199 cartesian_trajectory_planner_B.loop_ub_hm;
19200 cartesian_trajectory_planner_B.b_kstr_o++) {
19201 b_basename->data[cartesian_trajectory_planner_B.b_kstr_o] =
19202 newrobot->Base.NameInternal->
19203 data[cartesian_trajectory_planner_B.b_kstr_o];
19204 }
19205
19206 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19207 cartesian_trajectory_planner_B.bid_a = 0.0;
19208 } else {
19209 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
19210 cartesian_trajectory_planner_B.b_i_g = 0;
19211 exitg1 = false;
19212 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_g <=
19213 static_cast<int32_T>
19214 (cartesian_trajectory_planner_B.b_index) - 1)) {
19215 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_g];
19216 cartesian_trajectory_planner_B.b_kstr_o = b_basename->size[0] *
19217 b_basename->size[1];
19218 b_basename->size[0] = 1;
19219 b_basename->size[1] = body_0->NameInternal->size[1];
19220 cartes_emxEnsureCapacity_char_T(b_basename,
19221 cartesian_trajectory_planner_B.b_kstr_o);
19222 cartesian_trajectory_planner_B.loop_ub_hm = body_0->NameInternal->size[0]
19223 * body_0->NameInternal->size[1] - 1;
19224 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19225 cartesian_trajectory_planner_B.b_kstr_o <=
19226 cartesian_trajectory_planner_B.loop_ub_hm;
19227 cartesian_trajectory_planner_B.b_kstr_o++) {
19228 b_basename->data[cartesian_trajectory_planner_B.b_kstr_o] =
19229 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19230 }
19231
19232 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19233 cartesian_trajectory_planner_B.bid_a = static_cast<real_T>
19234 (cartesian_trajectory_planner_B.b_i_g) + 1.0;
19235 exitg1 = true;
19236 } else {
19237 cartesian_trajectory_planner_B.b_i_g++;
19238 }
19239 }
19240 }
19241
19242 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
19243 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);
19244 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
19245 - 1] = body_0;
19246 body_0->Index = cartesian_trajectory_planner_B.b_index;
19247 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_a;
19248 body_0->JointInternal->InTree = true;
19249 newrobot->NumBodies++;
19250 jnt = body_0->JointInternal;
19251 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19252 bname->size[0] = 1;
19253 bname->size[1] = jnt->Type->size[1];
19254 cartes_emxEnsureCapacity_char_T(bname,
19255 cartesian_trajectory_planner_B.b_kstr_o);
19256 cartesian_trajectory_planner_B.loop_ub_hm = jnt->Type->size[0] * jnt->
19257 Type->size[1] - 1;
19258 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19259 cartesian_trajectory_planner_B.b_kstr_o <=
19260 cartesian_trajectory_planner_B.loop_ub_hm;
19261 cartesian_trajectory_planner_B.b_kstr_o++) {
19262 bname->data[cartesian_trajectory_planner_B.b_kstr_o] = jnt->Type->
19263 data[cartesian_trajectory_planner_B.b_kstr_o];
19264 }
19265
19266 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19267 cartesian_trajectory_planner_B.b_kstr_o < 5;
19268 cartesian_trajectory_planner_B.b_kstr_o++) {
19269 cartesian_trajectory_planner_B.b_ku[cartesian_trajectory_planner_B.b_kstr_o]
19270 = tmp[cartesian_trajectory_planner_B.b_kstr_o];
19271 }
19272
19273 cartesian_trajectory_planner_B.b_bool_a = false;
19274 if (bname->size[1] == 5) {
19275 cartesian_trajectory_planner_B.b_kstr_o = 1;
19276 do {
19277 exitg2 = 0;
19278 if (cartesian_trajectory_planner_B.b_kstr_o - 1 < 5) {
19279 cartesian_trajectory_planner_B.loop_ub_hm =
19280 cartesian_trajectory_planner_B.b_kstr_o - 1;
19281 if (bname->data[cartesian_trajectory_planner_B.loop_ub_hm] !=
19282 cartesian_trajectory_planner_B.b_ku[cartesian_trajectory_planner_B.loop_ub_hm])
19283 {
19284 exitg2 = 1;
19285 } else {
19286 cartesian_trajectory_planner_B.b_kstr_o++;
19287 }
19288 } else {
19289 cartesian_trajectory_planner_B.b_bool_a = true;
19290 exitg2 = 1;
19291 }
19292 } while (exitg2 == 0);
19293 }
19294
19295 if (!cartesian_trajectory_planner_B.b_bool_a) {
19296 newrobot->NumNonFixedBodies++;
19297 jnt = body_0->JointInternal;
19298 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19299 (body_0->Index) - 1;
19300 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o] =
19301 newrobot->PositionNumber + 1.0;
19302 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 8] =
19303 newrobot->PositionNumber + jnt->PositionNumber;
19304 jnt = body_0->JointInternal;
19305 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19306 (body_0->Index) - 1;
19307 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o] =
19308 newrobot->VelocityNumber + 1.0;
19309 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 8] =
19310 newrobot->VelocityNumber + jnt->VelocityNumber;
19311 } else {
19312 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19313 (body_0->Index);
19314 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o - 1] =
19315 0.0;
19316 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 7] =
19317 -1.0;
19318 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19319 (body_0->Index);
19320 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o - 1] =
19321 0.0;
19322 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 7] =
19323 -1.0;
19324 }
19325
19326 jnt = body_0->JointInternal;
19327 newrobot->PositionNumber += jnt->PositionNumber;
19328 jnt = body_0->JointInternal;
19329 newrobot->VelocityNumber += jnt->VelocityNumber;
19330 }
19331
19332 if (8.0 <= rigidbodytree->NumBodies) {
19333 body = rigidbodytree->Bodies[7];
19334 cartesian_trajectory_planner_B.bid_a = body->ParentIndex;
19335 if (cartesian_trajectory_planner_B.bid_a > 0.0) {
19336 parent = rigidbodytree->Bodies[static_cast<int32_T>
19337 (cartesian_trajectory_planner_B.bid_a) - 1];
19338 } else {
19339 parent = &rigidbodytree->Base;
19340 }
19341
19342 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19343 bname->size[0] = 1;
19344 bname->size[1] = parent->NameInternal->size[1];
19345 cartes_emxEnsureCapacity_char_T(bname,
19346 cartesian_trajectory_planner_B.b_kstr_o);
19347 cartesian_trajectory_planner_B.loop_ub_hm = parent->NameInternal->size[0] *
19348 parent->NameInternal->size[1] - 1;
19349 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19350 cartesian_trajectory_planner_B.b_kstr_o <=
19351 cartesian_trajectory_planner_B.loop_ub_hm;
19352 cartesian_trajectory_planner_B.b_kstr_o++) {
19353 bname->data[cartesian_trajectory_planner_B.b_kstr_o] =
19354 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19355 }
19356
19357 cartesian_trajectory_planner_B.bid_a = -1.0;
19358 cartesian_trajectory_planner_B.b_kstr_o = b_basename->size[0] *
19359 b_basename->size[1];
19360 b_basename->size[0] = 1;
19361 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
19362 cartes_emxEnsureCapacity_char_T(b_basename,
19363 cartesian_trajectory_planner_B.b_kstr_o);
19364 cartesian_trajectory_planner_B.loop_ub_hm = newrobot->
19365 Base.NameInternal->size[0] * newrobot->Base.NameInternal->size[1] - 1;
19366 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19367 cartesian_trajectory_planner_B.b_kstr_o <=
19368 cartesian_trajectory_planner_B.loop_ub_hm;
19369 cartesian_trajectory_planner_B.b_kstr_o++) {
19370 b_basename->data[cartesian_trajectory_planner_B.b_kstr_o] =
19371 newrobot->Base.NameInternal->
19372 data[cartesian_trajectory_planner_B.b_kstr_o];
19373 }
19374
19375 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19376 cartesian_trajectory_planner_B.bid_a = 0.0;
19377 } else {
19378 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
19379 cartesian_trajectory_planner_B.b_i_g = 0;
19380 exitg1 = false;
19381 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_g <=
19382 static_cast<int32_T>
19383 (cartesian_trajectory_planner_B.b_index) - 1)) {
19384 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_g];
19385 cartesian_trajectory_planner_B.b_kstr_o = b_basename->size[0] *
19386 b_basename->size[1];
19387 b_basename->size[0] = 1;
19388 b_basename->size[1] = body_0->NameInternal->size[1];
19389 cartes_emxEnsureCapacity_char_T(b_basename,
19390 cartesian_trajectory_planner_B.b_kstr_o);
19391 cartesian_trajectory_planner_B.loop_ub_hm = body_0->NameInternal->size[0]
19392 * body_0->NameInternal->size[1] - 1;
19393 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19394 cartesian_trajectory_planner_B.b_kstr_o <=
19395 cartesian_trajectory_planner_B.loop_ub_hm;
19396 cartesian_trajectory_planner_B.b_kstr_o++) {
19397 b_basename->data[cartesian_trajectory_planner_B.b_kstr_o] =
19398 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_o];
19399 }
19400
19401 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19402 cartesian_trajectory_planner_B.bid_a = static_cast<real_T>
19403 (cartesian_trajectory_planner_B.b_i_g) + 1.0;
19404 exitg1 = true;
19405 } else {
19406 cartesian_trajectory_planner_B.b_i_g++;
19407 }
19408 }
19409 }
19410
19411 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
19412 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);
19413 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
19414 - 1] = body_0;
19415 body_0->Index = cartesian_trajectory_planner_B.b_index;
19416 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_a;
19417 body_0->JointInternal->InTree = true;
19418 newrobot->NumBodies++;
19419 jnt = body_0->JointInternal;
19420 cartesian_trajectory_planner_B.b_kstr_o = bname->size[0] * bname->size[1];
19421 bname->size[0] = 1;
19422 bname->size[1] = jnt->Type->size[1];
19423 cartes_emxEnsureCapacity_char_T(bname,
19424 cartesian_trajectory_planner_B.b_kstr_o);
19425 cartesian_trajectory_planner_B.loop_ub_hm = jnt->Type->size[0] * jnt->
19426 Type->size[1] - 1;
19427 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19428 cartesian_trajectory_planner_B.b_kstr_o <=
19429 cartesian_trajectory_planner_B.loop_ub_hm;
19430 cartesian_trajectory_planner_B.b_kstr_o++) {
19431 bname->data[cartesian_trajectory_planner_B.b_kstr_o] = jnt->Type->
19432 data[cartesian_trajectory_planner_B.b_kstr_o];
19433 }
19434
19435 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
19436 cartesian_trajectory_planner_B.b_kstr_o < 5;
19437 cartesian_trajectory_planner_B.b_kstr_o++) {
19438 cartesian_trajectory_planner_B.b_ku[cartesian_trajectory_planner_B.b_kstr_o]
19439 = tmp[cartesian_trajectory_planner_B.b_kstr_o];
19440 }
19441
19442 cartesian_trajectory_planner_B.b_bool_a = false;
19443 if (bname->size[1] == 5) {
19444 cartesian_trajectory_planner_B.b_kstr_o = 1;
19445 do {
19446 exitg2 = 0;
19447 if (cartesian_trajectory_planner_B.b_kstr_o - 1 < 5) {
19448 cartesian_trajectory_planner_B.loop_ub_hm =
19449 cartesian_trajectory_planner_B.b_kstr_o - 1;
19450 if (bname->data[cartesian_trajectory_planner_B.loop_ub_hm] !=
19451 cartesian_trajectory_planner_B.b_ku[cartesian_trajectory_planner_B.loop_ub_hm])
19452 {
19453 exitg2 = 1;
19454 } else {
19455 cartesian_trajectory_planner_B.b_kstr_o++;
19456 }
19457 } else {
19458 cartesian_trajectory_planner_B.b_bool_a = true;
19459 exitg2 = 1;
19460 }
19461 } while (exitg2 == 0);
19462 }
19463
19464 if (!cartesian_trajectory_planner_B.b_bool_a) {
19465 newrobot->NumNonFixedBodies++;
19466 jnt = body_0->JointInternal;
19467 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19468 (body_0->Index) - 1;
19469 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o] =
19470 newrobot->PositionNumber + 1.0;
19471 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 8] =
19472 newrobot->PositionNumber + jnt->PositionNumber;
19473 jnt = body_0->JointInternal;
19474 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19475 (body_0->Index) - 1;
19476 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o] =
19477 newrobot->VelocityNumber + 1.0;
19478 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 8] =
19479 newrobot->VelocityNumber + jnt->VelocityNumber;
19480 } else {
19481 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19482 (body_0->Index);
19483 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o - 1] =
19484 0.0;
19485 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 7] =
19486 -1.0;
19487 cartesian_trajectory_planner_B.b_kstr_o = static_cast<int32_T>
19488 (body_0->Index);
19489 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o - 1] =
19490 0.0;
19491 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_o + 7] =
19492 -1.0;
19493 }
19494
19495 jnt = body_0->JointInternal;
19496 newrobot->PositionNumber += jnt->PositionNumber;
19497 jnt = body_0->JointInternal;
19498 newrobot->VelocityNumber += jnt->VelocityNumber;
19499 }
19500
19501 cartesian_trajec_emxFree_char_T(&bname);
19502 cartesian_trajec_emxFree_char_T(&b_basename);
19503 obj->RigidBodyTreeInternal = newrobot;
19504}
19505
19506static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
19507 (h_robotics_core_internal_Damp_T *obj)
19508{
19509 h_robotics_core_internal_Damp_T *b_obj;
19510 int32_T i;
19511 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
19512 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
19513
19514 b_obj = obj;
19515 obj->MaxNumIteration = 1500.0;
19516 obj->MaxTime = 10.0;
19517 obj->GradientTolerance = 1.0E-7;
19518 obj->SolutionTolerance = 1.0E-6;
19519 obj->ArmijoRuleBeta = 0.4;
19520 obj->ArmijoRuleSigma = 1.0E-5;
19521 obj->ConstraintsOn = true;
19522 obj->RandomRestart = true;
19523 obj->StepTolerance = 1.0E-14;
19524 for (i = 0; i < 22; i++) {
19525 obj->Name[i] = tmp[i];
19526 }
19527
19528 obj->ConstraintMatrix->size[0] = 0;
19529 obj->ConstraintMatrix->size[1] = 0;
19530 obj->ConstraintBound->size[0] = 0;
19531 obj->TimeObj.StartTime = -1.0;
19532 obj->TimeObjInternal.StartTime = -1.0;
19533 return b_obj;
19534}
19535
19536static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
19537 *pStruct)
19538{
19539 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
19540 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
19541}
19542
19543static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
19544 *pStruct)
19545{
19546 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
19547 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
19548}
19549
19550static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
19551 *pStruct)
19552{
19553 emxInitStruct_o_robotics_manip_(&pStruct->Base);
19554}
19555
19556static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
19557 *pStruct)
19558{
19559 emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);
19560}
19561
19562static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
19563 *pStruct)
19564{
19565 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
19566 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
19567}
19568
19569static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
19570 (n_robotics_manip_internal_Rig_T *obj)
19571{
19572 n_robotics_manip_internal_Rig_T *b_obj;
19573 int8_T msubspace_data[36];
19574 emxArray_char_T_cartesian_tra_T *switch_expression;
19575 boolean_T b_bool;
19576 int32_T b_kstr;
19577 char_T b[8];
19578 char_T b_0[9];
19579 int32_T loop_ub;
19580 int8_T tmp[6];
19581 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
19582 'l', 'i', 'n', 'k' };
19583
19584 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
19585
19586 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19587
19588 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19589
19590 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19591 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19592
19593 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19594 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19595
19596 int32_T exitg1;
19597 b_obj = obj;
19598 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19599 obj->NameInternal->size[0] = 1;
19600 obj->NameInternal->size[1] = 13;
19601 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19602 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
19603 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19604 }
19605
19606 obj->ParentIndex = 0.0;
19607 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19608 obj->JointInternal.Type->size[0] = 1;
19609 obj->JointInternal.Type->size[1] = 5;
19610 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
19611 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
19612 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
19613 }
19614
19615 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
19616 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19617 switch_expression->size[0] = 1;
19618 switch_expression->size[1] = obj->JointInternal.Type->size[1];
19619 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19620 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
19621 - 1;
19622 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19623 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
19624 }
19625
19626 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19627 b[b_kstr] = tmp_2[b_kstr];
19628 }
19629
19630 b_bool = false;
19631 if (switch_expression->size[1] == 8) {
19632 b_kstr = 1;
19633 do {
19634 exitg1 = 0;
19635 if (b_kstr - 1 < 8) {
19636 loop_ub = b_kstr - 1;
19637 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19638 exitg1 = 1;
19639 } else {
19640 b_kstr++;
19641 }
19642 } else {
19643 b_bool = true;
19644 exitg1 = 1;
19645 }
19646 } while (exitg1 == 0);
19647 }
19648
19649 if (b_bool) {
19650 b_kstr = 0;
19651 } else {
19652 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19653 b_0[b_kstr] = tmp_3[b_kstr];
19654 }
19655
19656 b_bool = false;
19657 if (switch_expression->size[1] == 9) {
19658 b_kstr = 1;
19659 do {
19660 exitg1 = 0;
19661 if (b_kstr - 1 < 9) {
19662 loop_ub = b_kstr - 1;
19663 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19664 exitg1 = 1;
19665 } else {
19666 b_kstr++;
19667 }
19668 } else {
19669 b_bool = true;
19670 exitg1 = 1;
19671 }
19672 } while (exitg1 == 0);
19673 }
19674
19675 if (b_bool) {
19676 b_kstr = 1;
19677 } else {
19678 b_kstr = -1;
19679 }
19680 }
19681
19682 cartesian_trajec_emxFree_char_T(&switch_expression);
19683 switch (b_kstr) {
19684 case 0:
19685 tmp[0] = 0;
19686 tmp[1] = 0;
19687 tmp[2] = 1;
19688 tmp[3] = 0;
19689 tmp[4] = 0;
19690 tmp[5] = 0;
19691 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19692 msubspace_data[b_kstr] = tmp[b_kstr];
19693 }
19694
19695 obj->JointInternal.PositionNumber = 1.0;
19696 obj->JointInternal.JointAxisInternal[0] = 0.0;
19697 obj->JointInternal.JointAxisInternal[1] = 0.0;
19698 obj->JointInternal.JointAxisInternal[2] = 1.0;
19699 break;
19700
19701 case 1:
19702 tmp[0] = 0;
19703 tmp[1] = 0;
19704 tmp[2] = 0;
19705 tmp[3] = 0;
19706 tmp[4] = 0;
19707 tmp[5] = 1;
19708 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19709 msubspace_data[b_kstr] = tmp[b_kstr];
19710 }
19711
19712 obj->JointInternal.PositionNumber = 1.0;
19713 obj->JointInternal.JointAxisInternal[0] = 0.0;
19714 obj->JointInternal.JointAxisInternal[1] = 0.0;
19715 obj->JointInternal.JointAxisInternal[2] = 1.0;
19716 break;
19717
19718 default:
19719 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19720 msubspace_data[b_kstr] = 0;
19721 }
19722
19723 obj->JointInternal.PositionNumber = 0.0;
19724 obj->JointInternal.JointAxisInternal[0] = 0.0;
19725 obj->JointInternal.JointAxisInternal[1] = 0.0;
19726 obj->JointInternal.JointAxisInternal[2] = 0.0;
19727 break;
19728 }
19729
19730 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19731 obj->JointInternal.MotionSubspace->size[1];
19732 obj->JointInternal.MotionSubspace->size[0] = 6;
19733 obj->JointInternal.MotionSubspace->size[1] = 1;
19734 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19735 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19736 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19737 }
19738
19739 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19740 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
19741 }
19742
19743 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19744 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
19745 }
19746
19747 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19748 obj->JointInternal.MotionSubspace->size[1];
19749 obj->JointInternal.MotionSubspace->size[0] = 6;
19750 obj->JointInternal.MotionSubspace->size[1] = 1;
19751 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19752 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19753 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
19754 }
19755
19756 obj->JointInternal.JointAxisInternal[0] = 0.0;
19757 obj->JointInternal.JointAxisInternal[1] = 0.0;
19758 obj->JointInternal.JointAxisInternal[2] = 0.0;
19759 return b_obj;
19760}
19761
19762static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_a
19763 (n_robotics_manip_internal_Rig_T *obj)
19764{
19765 n_robotics_manip_internal_Rig_T *b_obj;
19766 int8_T msubspace_data[36];
19767 emxArray_char_T_cartesian_tra_T *switch_expression;
19768 boolean_T b_bool;
19769 int32_T b_kstr;
19770 char_T b[8];
19771 char_T b_0[9];
19772 int32_T loop_ub;
19773 int8_T tmp[6];
19774 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
19775 '1' };
19776
19777 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19778
19779 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19780
19781 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19782 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
19783
19784 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19785 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19786
19787 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19788 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19789 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
19790
19791 int32_T exitg1;
19792 b_obj = obj;
19793 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19794 obj->NameInternal->size[0] = 1;
19795 obj->NameInternal->size[1] = 10;
19796 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19797 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
19798 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19799 }
19800
19801 obj->ParentIndex = 1.0;
19802 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19803 obj->JointInternal.Type->size[0] = 1;
19804 obj->JointInternal.Type->size[1] = 8;
19805 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
19806 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19807 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
19808 }
19809
19810 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
19811 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19812 switch_expression->size[0] = 1;
19813 switch_expression->size[1] = obj->JointInternal.Type->size[1];
19814 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19815 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
19816 - 1;
19817 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19818 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
19819 }
19820
19821 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19822 b[b_kstr] = tmp_1[b_kstr];
19823 }
19824
19825 b_bool = false;
19826 if (switch_expression->size[1] == 8) {
19827 b_kstr = 1;
19828 do {
19829 exitg1 = 0;
19830 if (b_kstr - 1 < 8) {
19831 loop_ub = b_kstr - 1;
19832 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19833 exitg1 = 1;
19834 } else {
19835 b_kstr++;
19836 }
19837 } else {
19838 b_bool = true;
19839 exitg1 = 1;
19840 }
19841 } while (exitg1 == 0);
19842 }
19843
19844 if (b_bool) {
19845 b_kstr = 0;
19846 } else {
19847 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19848 b_0[b_kstr] = tmp_2[b_kstr];
19849 }
19850
19851 b_bool = false;
19852 if (switch_expression->size[1] == 9) {
19853 b_kstr = 1;
19854 do {
19855 exitg1 = 0;
19856 if (b_kstr - 1 < 9) {
19857 loop_ub = b_kstr - 1;
19858 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19859 exitg1 = 1;
19860 } else {
19861 b_kstr++;
19862 }
19863 } else {
19864 b_bool = true;
19865 exitg1 = 1;
19866 }
19867 } while (exitg1 == 0);
19868 }
19869
19870 if (b_bool) {
19871 b_kstr = 1;
19872 } else {
19873 b_kstr = -1;
19874 }
19875 }
19876
19877 cartesian_trajec_emxFree_char_T(&switch_expression);
19878 switch (b_kstr) {
19879 case 0:
19880 tmp[0] = 0;
19881 tmp[1] = 0;
19882 tmp[2] = 1;
19883 tmp[3] = 0;
19884 tmp[4] = 0;
19885 tmp[5] = 0;
19886 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19887 msubspace_data[b_kstr] = tmp[b_kstr];
19888 }
19889
19890 obj->JointInternal.PositionNumber = 1.0;
19891 obj->JointInternal.JointAxisInternal[0] = 0.0;
19892 obj->JointInternal.JointAxisInternal[1] = 0.0;
19893 obj->JointInternal.JointAxisInternal[2] = 1.0;
19894 break;
19895
19896 case 1:
19897 tmp[0] = 0;
19898 tmp[1] = 0;
19899 tmp[2] = 0;
19900 tmp[3] = 0;
19901 tmp[4] = 0;
19902 tmp[5] = 1;
19903 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19904 msubspace_data[b_kstr] = tmp[b_kstr];
19905 }
19906
19907 obj->JointInternal.PositionNumber = 1.0;
19908 obj->JointInternal.JointAxisInternal[0] = 0.0;
19909 obj->JointInternal.JointAxisInternal[1] = 0.0;
19910 obj->JointInternal.JointAxisInternal[2] = 1.0;
19911 break;
19912
19913 default:
19914 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19915 msubspace_data[b_kstr] = 0;
19916 }
19917
19918 obj->JointInternal.PositionNumber = 0.0;
19919 obj->JointInternal.JointAxisInternal[0] = 0.0;
19920 obj->JointInternal.JointAxisInternal[1] = 0.0;
19921 obj->JointInternal.JointAxisInternal[2] = 0.0;
19922 break;
19923 }
19924
19925 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19926 obj->JointInternal.MotionSubspace->size[1];
19927 obj->JointInternal.MotionSubspace->size[0] = 6;
19928 obj->JointInternal.MotionSubspace->size[1] = 1;
19929 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19930 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19931 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19932 }
19933
19934 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19935 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
19936 }
19937
19938 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19939 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
19940 }
19941
19942 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19943 obj->JointInternal.MotionSubspace->size[1];
19944 obj->JointInternal.MotionSubspace->size[0] = 6;
19945 obj->JointInternal.MotionSubspace->size[1] = 1;
19946 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19947 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19948 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
19949 }
19950
19951 obj->JointInternal.JointAxisInternal[0] = 0.0;
19952 obj->JointInternal.JointAxisInternal[1] = 0.0;
19953 obj->JointInternal.JointAxisInternal[2] = 1.0;
19954 return b_obj;
19955}
19956
19957static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_as
19958 (n_robotics_manip_internal_Rig_T *obj)
19959{
19960 n_robotics_manip_internal_Rig_T *b_obj;
19961 int8_T msubspace_data[36];
19962 emxArray_char_T_cartesian_tra_T *switch_expression;
19963 boolean_T b_bool;
19964 int32_T b_kstr;
19965 char_T b[8];
19966 char_T b_0[9];
19967 int32_T loop_ub;
19968 int8_T tmp[6];
19969 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
19970 '2' };
19971
19972 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19973
19974 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19975
19976 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
19977 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
19978 0.0, 0.0, 0.0, 1.0 };
19979
19980 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19981 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19982
19983 static const real_T tmp_5[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19984 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19985 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
19986
19987 int32_T exitg1;
19988 b_obj = obj;
19989 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19990 obj->NameInternal->size[0] = 1;
19991 obj->NameInternal->size[1] = 10;
19992 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19993 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
19994 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19995 }
19996
19997 obj->ParentIndex = 2.0;
19998 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19999 obj->JointInternal.Type->size[0] = 1;
20000 obj->JointInternal.Type->size[1] = 8;
20001 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20002 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20003 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20004 }
20005
20006 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20007 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20008 switch_expression->size[0] = 1;
20009 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20010 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20011 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20012 - 1;
20013 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20014 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20015 }
20016
20017 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20018 b[b_kstr] = tmp_1[b_kstr];
20019 }
20020
20021 b_bool = false;
20022 if (switch_expression->size[1] == 8) {
20023 b_kstr = 1;
20024 do {
20025 exitg1 = 0;
20026 if (b_kstr - 1 < 8) {
20027 loop_ub = b_kstr - 1;
20028 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20029 exitg1 = 1;
20030 } else {
20031 b_kstr++;
20032 }
20033 } else {
20034 b_bool = true;
20035 exitg1 = 1;
20036 }
20037 } while (exitg1 == 0);
20038 }
20039
20040 if (b_bool) {
20041 b_kstr = 0;
20042 } else {
20043 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20044 b_0[b_kstr] = tmp_2[b_kstr];
20045 }
20046
20047 b_bool = false;
20048 if (switch_expression->size[1] == 9) {
20049 b_kstr = 1;
20050 do {
20051 exitg1 = 0;
20052 if (b_kstr - 1 < 9) {
20053 loop_ub = b_kstr - 1;
20054 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20055 exitg1 = 1;
20056 } else {
20057 b_kstr++;
20058 }
20059 } else {
20060 b_bool = true;
20061 exitg1 = 1;
20062 }
20063 } while (exitg1 == 0);
20064 }
20065
20066 if (b_bool) {
20067 b_kstr = 1;
20068 } else {
20069 b_kstr = -1;
20070 }
20071 }
20072
20073 cartesian_trajec_emxFree_char_T(&switch_expression);
20074 switch (b_kstr) {
20075 case 0:
20076 tmp[0] = 0;
20077 tmp[1] = 0;
20078 tmp[2] = 1;
20079 tmp[3] = 0;
20080 tmp[4] = 0;
20081 tmp[5] = 0;
20082 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20083 msubspace_data[b_kstr] = tmp[b_kstr];
20084 }
20085
20086 obj->JointInternal.PositionNumber = 1.0;
20087 obj->JointInternal.JointAxisInternal[0] = 0.0;
20088 obj->JointInternal.JointAxisInternal[1] = 0.0;
20089 obj->JointInternal.JointAxisInternal[2] = 1.0;
20090 break;
20091
20092 case 1:
20093 tmp[0] = 0;
20094 tmp[1] = 0;
20095 tmp[2] = 0;
20096 tmp[3] = 0;
20097 tmp[4] = 0;
20098 tmp[5] = 1;
20099 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20100 msubspace_data[b_kstr] = tmp[b_kstr];
20101 }
20102
20103 obj->JointInternal.PositionNumber = 1.0;
20104 obj->JointInternal.JointAxisInternal[0] = 0.0;
20105 obj->JointInternal.JointAxisInternal[1] = 0.0;
20106 obj->JointInternal.JointAxisInternal[2] = 1.0;
20107 break;
20108
20109 default:
20110 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20111 msubspace_data[b_kstr] = 0;
20112 }
20113
20114 obj->JointInternal.PositionNumber = 0.0;
20115 obj->JointInternal.JointAxisInternal[0] = 0.0;
20116 obj->JointInternal.JointAxisInternal[1] = 0.0;
20117 obj->JointInternal.JointAxisInternal[2] = 0.0;
20118 break;
20119 }
20120
20121 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20122 obj->JointInternal.MotionSubspace->size[1];
20123 obj->JointInternal.MotionSubspace->size[0] = 6;
20124 obj->JointInternal.MotionSubspace->size[1] = 1;
20125 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20126 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20127 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20128 }
20129
20130 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20131 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20132 }
20133
20134 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20135 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20136 }
20137
20138 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20139 obj->JointInternal.MotionSubspace->size[1];
20140 obj->JointInternal.MotionSubspace->size[0] = 6;
20141 obj->JointInternal.MotionSubspace->size[1] = 1;
20142 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20143 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20144 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20145 }
20146
20147 obj->JointInternal.JointAxisInternal[0] = 0.0;
20148 obj->JointInternal.JointAxisInternal[1] = 0.0;
20149 obj->JointInternal.JointAxisInternal[2] = -1.0;
20150 return b_obj;
20151}
20152
20153static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_ast
20154 (n_robotics_manip_internal_Rig_T *obj)
20155{
20156 n_robotics_manip_internal_Rig_T *b_obj;
20157 int8_T msubspace_data[36];
20158 emxArray_char_T_cartesian_tra_T *switch_expression;
20159 boolean_T b_bool;
20160 int32_T b_kstr;
20161 char_T b[8];
20162 char_T b_0[9];
20163 int32_T loop_ub;
20164 int8_T tmp[6];
20165 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20166 '3' };
20167
20168 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20169
20170 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20171
20172 static const real_T tmp_3[16] = { 1.0, 2.0682310711021444E-13,
20173 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
20174 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
20175 1.0 };
20176
20177 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20178 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20179
20180 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20181 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20182 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20183
20184 int32_T exitg1;
20185 b_obj = obj;
20186 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20187 obj->NameInternal->size[0] = 1;
20188 obj->NameInternal->size[1] = 10;
20189 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20190 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20191 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20192 }
20193
20194 obj->ParentIndex = 3.0;
20195 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20196 obj->JointInternal.Type->size[0] = 1;
20197 obj->JointInternal.Type->size[1] = 8;
20198 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20199 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20200 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20201 }
20202
20203 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20204 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20205 switch_expression->size[0] = 1;
20206 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20207 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20208 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20209 - 1;
20210 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20211 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20212 }
20213
20214 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20215 b[b_kstr] = tmp_1[b_kstr];
20216 }
20217
20218 b_bool = false;
20219 if (switch_expression->size[1] == 8) {
20220 b_kstr = 1;
20221 do {
20222 exitg1 = 0;
20223 if (b_kstr - 1 < 8) {
20224 loop_ub = b_kstr - 1;
20225 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20226 exitg1 = 1;
20227 } else {
20228 b_kstr++;
20229 }
20230 } else {
20231 b_bool = true;
20232 exitg1 = 1;
20233 }
20234 } while (exitg1 == 0);
20235 }
20236
20237 if (b_bool) {
20238 b_kstr = 0;
20239 } else {
20240 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20241 b_0[b_kstr] = tmp_2[b_kstr];
20242 }
20243
20244 b_bool = false;
20245 if (switch_expression->size[1] == 9) {
20246 b_kstr = 1;
20247 do {
20248 exitg1 = 0;
20249 if (b_kstr - 1 < 9) {
20250 loop_ub = b_kstr - 1;
20251 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20252 exitg1 = 1;
20253 } else {
20254 b_kstr++;
20255 }
20256 } else {
20257 b_bool = true;
20258 exitg1 = 1;
20259 }
20260 } while (exitg1 == 0);
20261 }
20262
20263 if (b_bool) {
20264 b_kstr = 1;
20265 } else {
20266 b_kstr = -1;
20267 }
20268 }
20269
20270 cartesian_trajec_emxFree_char_T(&switch_expression);
20271 switch (b_kstr) {
20272 case 0:
20273 tmp[0] = 0;
20274 tmp[1] = 0;
20275 tmp[2] = 1;
20276 tmp[3] = 0;
20277 tmp[4] = 0;
20278 tmp[5] = 0;
20279 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20280 msubspace_data[b_kstr] = tmp[b_kstr];
20281 }
20282
20283 obj->JointInternal.PositionNumber = 1.0;
20284 obj->JointInternal.JointAxisInternal[0] = 0.0;
20285 obj->JointInternal.JointAxisInternal[1] = 0.0;
20286 obj->JointInternal.JointAxisInternal[2] = 1.0;
20287 break;
20288
20289 case 1:
20290 tmp[0] = 0;
20291 tmp[1] = 0;
20292 tmp[2] = 0;
20293 tmp[3] = 0;
20294 tmp[4] = 0;
20295 tmp[5] = 1;
20296 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20297 msubspace_data[b_kstr] = tmp[b_kstr];
20298 }
20299
20300 obj->JointInternal.PositionNumber = 1.0;
20301 obj->JointInternal.JointAxisInternal[0] = 0.0;
20302 obj->JointInternal.JointAxisInternal[1] = 0.0;
20303 obj->JointInternal.JointAxisInternal[2] = 1.0;
20304 break;
20305
20306 default:
20307 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20308 msubspace_data[b_kstr] = 0;
20309 }
20310
20311 obj->JointInternal.PositionNumber = 0.0;
20312 obj->JointInternal.JointAxisInternal[0] = 0.0;
20313 obj->JointInternal.JointAxisInternal[1] = 0.0;
20314 obj->JointInternal.JointAxisInternal[2] = 0.0;
20315 break;
20316 }
20317
20318 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20319 obj->JointInternal.MotionSubspace->size[1];
20320 obj->JointInternal.MotionSubspace->size[0] = 6;
20321 obj->JointInternal.MotionSubspace->size[1] = 1;
20322 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20323 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20324 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20325 }
20326
20327 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20328 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20329 }
20330
20331 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20332 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20333 }
20334
20335 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20336 obj->JointInternal.MotionSubspace->size[1];
20337 obj->JointInternal.MotionSubspace->size[0] = 6;
20338 obj->JointInternal.MotionSubspace->size[1] = 1;
20339 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20340 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20341 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20342 }
20343
20344 obj->JointInternal.JointAxisInternal[0] = 0.0;
20345 obj->JointInternal.JointAxisInternal[1] = 0.0;
20346 obj->JointInternal.JointAxisInternal[2] = 1.0;
20347 return b_obj;
20348}
20349
20350static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_astw
20351 (n_robotics_manip_internal_Rig_T *obj)
20352{
20353 n_robotics_manip_internal_Rig_T *b_obj;
20354 int8_T msubspace_data[36];
20355 emxArray_char_T_cartesian_tra_T *switch_expression;
20356 boolean_T b_bool;
20357 int32_T b_kstr;
20358 char_T b[8];
20359 char_T b_0[9];
20360 int32_T loop_ub;
20361 int8_T tmp[6];
20362 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20363 '4' };
20364
20365 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20366
20367 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20368
20369 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
20370 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
20371 0.0, -0.268, 0.0, 1.0 };
20372
20373 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20374 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20375
20376 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20377 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20378 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20379
20380 int32_T exitg1;
20381 b_obj = obj;
20382 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20383 obj->NameInternal->size[0] = 1;
20384 obj->NameInternal->size[1] = 10;
20385 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20386 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20387 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20388 }
20389
20390 obj->ParentIndex = 4.0;
20391 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20392 obj->JointInternal.Type->size[0] = 1;
20393 obj->JointInternal.Type->size[1] = 8;
20394 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20395 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20396 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20397 }
20398
20399 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20400 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20401 switch_expression->size[0] = 1;
20402 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20403 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20404 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20405 - 1;
20406 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20407 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20408 }
20409
20410 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20411 b[b_kstr] = tmp_1[b_kstr];
20412 }
20413
20414 b_bool = false;
20415 if (switch_expression->size[1] == 8) {
20416 b_kstr = 1;
20417 do {
20418 exitg1 = 0;
20419 if (b_kstr - 1 < 8) {
20420 loop_ub = b_kstr - 1;
20421 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20422 exitg1 = 1;
20423 } else {
20424 b_kstr++;
20425 }
20426 } else {
20427 b_bool = true;
20428 exitg1 = 1;
20429 }
20430 } while (exitg1 == 0);
20431 }
20432
20433 if (b_bool) {
20434 b_kstr = 0;
20435 } else {
20436 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20437 b_0[b_kstr] = tmp_2[b_kstr];
20438 }
20439
20440 b_bool = false;
20441 if (switch_expression->size[1] == 9) {
20442 b_kstr = 1;
20443 do {
20444 exitg1 = 0;
20445 if (b_kstr - 1 < 9) {
20446 loop_ub = b_kstr - 1;
20447 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20448 exitg1 = 1;
20449 } else {
20450 b_kstr++;
20451 }
20452 } else {
20453 b_bool = true;
20454 exitg1 = 1;
20455 }
20456 } while (exitg1 == 0);
20457 }
20458
20459 if (b_bool) {
20460 b_kstr = 1;
20461 } else {
20462 b_kstr = -1;
20463 }
20464 }
20465
20466 cartesian_trajec_emxFree_char_T(&switch_expression);
20467 switch (b_kstr) {
20468 case 0:
20469 tmp[0] = 0;
20470 tmp[1] = 0;
20471 tmp[2] = 1;
20472 tmp[3] = 0;
20473 tmp[4] = 0;
20474 tmp[5] = 0;
20475 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20476 msubspace_data[b_kstr] = tmp[b_kstr];
20477 }
20478
20479 obj->JointInternal.PositionNumber = 1.0;
20480 obj->JointInternal.JointAxisInternal[0] = 0.0;
20481 obj->JointInternal.JointAxisInternal[1] = 0.0;
20482 obj->JointInternal.JointAxisInternal[2] = 1.0;
20483 break;
20484
20485 case 1:
20486 tmp[0] = 0;
20487 tmp[1] = 0;
20488 tmp[2] = 0;
20489 tmp[3] = 0;
20490 tmp[4] = 0;
20491 tmp[5] = 1;
20492 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20493 msubspace_data[b_kstr] = tmp[b_kstr];
20494 }
20495
20496 obj->JointInternal.PositionNumber = 1.0;
20497 obj->JointInternal.JointAxisInternal[0] = 0.0;
20498 obj->JointInternal.JointAxisInternal[1] = 0.0;
20499 obj->JointInternal.JointAxisInternal[2] = 1.0;
20500 break;
20501
20502 default:
20503 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20504 msubspace_data[b_kstr] = 0;
20505 }
20506
20507 obj->JointInternal.PositionNumber = 0.0;
20508 obj->JointInternal.JointAxisInternal[0] = 0.0;
20509 obj->JointInternal.JointAxisInternal[1] = 0.0;
20510 obj->JointInternal.JointAxisInternal[2] = 0.0;
20511 break;
20512 }
20513
20514 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20515 obj->JointInternal.MotionSubspace->size[1];
20516 obj->JointInternal.MotionSubspace->size[0] = 6;
20517 obj->JointInternal.MotionSubspace->size[1] = 1;
20518 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20519 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20520 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20521 }
20522
20523 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20524 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20525 }
20526
20527 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20528 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20529 }
20530
20531 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20532 obj->JointInternal.MotionSubspace->size[1];
20533 obj->JointInternal.MotionSubspace->size[0] = 6;
20534 obj->JointInternal.MotionSubspace->size[1] = 1;
20535 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20536 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20537 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20538 }
20539
20540 obj->JointInternal.JointAxisInternal[0] = 0.0;
20541 obj->JointInternal.JointAxisInternal[1] = 0.0;
20542 obj->JointInternal.JointAxisInternal[2] = 1.0;
20543 return b_obj;
20544}
20545
20546static n_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_astwh
20547 (n_robotics_manip_internal_Rig_T *obj)
20548{
20549 n_robotics_manip_internal_Rig_T *b_obj;
20550 int8_T msubspace_data[36];
20551 emxArray_char_T_cartesian_tra_T *switch_expression;
20552 boolean_T b_bool;
20553 int32_T b_kstr;
20554 char_T b[8];
20555 char_T b_0[9];
20556 int32_T loop_ub;
20557 int8_T tmp[6];
20558 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20559 '5' };
20560
20561 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20562
20563 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20564
20565 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20566 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20567
20568 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20569 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20570
20571 static const real_T tmp_5[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20572 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20573 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20574
20575 int32_T exitg1;
20576 b_obj = obj;
20577 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20578 obj->NameInternal->size[0] = 1;
20579 obj->NameInternal->size[1] = 10;
20580 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20581 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20582 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20583 }
20584
20585 obj->ParentIndex = 5.0;
20586 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20587 obj->JointInternal.Type->size[0] = 1;
20588 obj->JointInternal.Type->size[1] = 8;
20589 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20590 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20591 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20592 }
20593
20594 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20595 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20596 switch_expression->size[0] = 1;
20597 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20598 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20599 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20600 - 1;
20601 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20602 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20603 }
20604
20605 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20606 b[b_kstr] = tmp_1[b_kstr];
20607 }
20608
20609 b_bool = false;
20610 if (switch_expression->size[1] == 8) {
20611 b_kstr = 1;
20612 do {
20613 exitg1 = 0;
20614 if (b_kstr - 1 < 8) {
20615 loop_ub = b_kstr - 1;
20616 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20617 exitg1 = 1;
20618 } else {
20619 b_kstr++;
20620 }
20621 } else {
20622 b_bool = true;
20623 exitg1 = 1;
20624 }
20625 } while (exitg1 == 0);
20626 }
20627
20628 if (b_bool) {
20629 b_kstr = 0;
20630 } else {
20631 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20632 b_0[b_kstr] = tmp_2[b_kstr];
20633 }
20634
20635 b_bool = false;
20636 if (switch_expression->size[1] == 9) {
20637 b_kstr = 1;
20638 do {
20639 exitg1 = 0;
20640 if (b_kstr - 1 < 9) {
20641 loop_ub = b_kstr - 1;
20642 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20643 exitg1 = 1;
20644 } else {
20645 b_kstr++;
20646 }
20647 } else {
20648 b_bool = true;
20649 exitg1 = 1;
20650 }
20651 } while (exitg1 == 0);
20652 }
20653
20654 if (b_bool) {
20655 b_kstr = 1;
20656 } else {
20657 b_kstr = -1;
20658 }
20659 }
20660
20661 cartesian_trajec_emxFree_char_T(&switch_expression);
20662 switch (b_kstr) {
20663 case 0:
20664 tmp[0] = 0;
20665 tmp[1] = 0;
20666 tmp[2] = 1;
20667 tmp[3] = 0;
20668 tmp[4] = 0;
20669 tmp[5] = 0;
20670 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20671 msubspace_data[b_kstr] = tmp[b_kstr];
20672 }
20673
20674 obj->JointInternal.PositionNumber = 1.0;
20675 obj->JointInternal.JointAxisInternal[0] = 0.0;
20676 obj->JointInternal.JointAxisInternal[1] = 0.0;
20677 obj->JointInternal.JointAxisInternal[2] = 1.0;
20678 break;
20679
20680 case 1:
20681 tmp[0] = 0;
20682 tmp[1] = 0;
20683 tmp[2] = 0;
20684 tmp[3] = 0;
20685 tmp[4] = 0;
20686 tmp[5] = 1;
20687 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20688 msubspace_data[b_kstr] = tmp[b_kstr];
20689 }
20690
20691 obj->JointInternal.PositionNumber = 1.0;
20692 obj->JointInternal.JointAxisInternal[0] = 0.0;
20693 obj->JointInternal.JointAxisInternal[1] = 0.0;
20694 obj->JointInternal.JointAxisInternal[2] = 1.0;
20695 break;
20696
20697 default:
20698 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20699 msubspace_data[b_kstr] = 0;
20700 }
20701
20702 obj->JointInternal.PositionNumber = 0.0;
20703 obj->JointInternal.JointAxisInternal[0] = 0.0;
20704 obj->JointInternal.JointAxisInternal[1] = 0.0;
20705 obj->JointInternal.JointAxisInternal[2] = 0.0;
20706 break;
20707 }
20708
20709 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20710 obj->JointInternal.MotionSubspace->size[1];
20711 obj->JointInternal.MotionSubspace->size[0] = 6;
20712 obj->JointInternal.MotionSubspace->size[1] = 1;
20713 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20714 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20715 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20716 }
20717
20718 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20719 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20720 }
20721
20722 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20723 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20724 }
20725
20726 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20727 obj->JointInternal.MotionSubspace->size[1];
20728 obj->JointInternal.MotionSubspace->size[0] = 6;
20729 obj->JointInternal.MotionSubspace->size[1] = 1;
20730 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20731 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20732 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20733 }
20734
20735 obj->JointInternal.JointAxisInternal[0] = 0.0;
20736 obj->JointInternal.JointAxisInternal[1] = 1.0;
20737 obj->JointInternal.JointAxisInternal[2] = 0.0;
20738 return b_obj;
20739}
20740
20741static n_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_astwhq
20742 (n_robotics_manip_internal_Rig_T *obj)
20743{
20744 n_robotics_manip_internal_Rig_T *b_obj;
20745 int8_T msubspace_data[36];
20746 emxArray_char_T_cartesian_tra_T *switch_expression;
20747 boolean_T b_bool;
20748 int32_T b_kstr;
20749 char_T b[8];
20750 char_T b_0[9];
20751 int32_T loop_ub;
20752 int8_T tmp[6];
20753 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20754 '6' };
20755
20756 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20757
20758 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20759
20760 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20761 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
20762
20763 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20764 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20765
20766 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20767 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20768 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20769
20770 int32_T exitg1;
20771 b_obj = obj;
20772 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20773 obj->NameInternal->size[0] = 1;
20774 obj->NameInternal->size[1] = 10;
20775 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20776 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20777 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20778 }
20779
20780 obj->ParentIndex = 6.0;
20781 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20782 obj->JointInternal.Type->size[0] = 1;
20783 obj->JointInternal.Type->size[1] = 8;
20784 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20785 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20786 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20787 }
20788
20789 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20790 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20791 switch_expression->size[0] = 1;
20792 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20793 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20794 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20795 - 1;
20796 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20797 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20798 }
20799
20800 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20801 b[b_kstr] = tmp_1[b_kstr];
20802 }
20803
20804 b_bool = false;
20805 if (switch_expression->size[1] == 8) {
20806 b_kstr = 1;
20807 do {
20808 exitg1 = 0;
20809 if (b_kstr - 1 < 8) {
20810 loop_ub = b_kstr - 1;
20811 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20812 exitg1 = 1;
20813 } else {
20814 b_kstr++;
20815 }
20816 } else {
20817 b_bool = true;
20818 exitg1 = 1;
20819 }
20820 } while (exitg1 == 0);
20821 }
20822
20823 if (b_bool) {
20824 b_kstr = 0;
20825 } else {
20826 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20827 b_0[b_kstr] = tmp_2[b_kstr];
20828 }
20829
20830 b_bool = false;
20831 if (switch_expression->size[1] == 9) {
20832 b_kstr = 1;
20833 do {
20834 exitg1 = 0;
20835 if (b_kstr - 1 < 9) {
20836 loop_ub = b_kstr - 1;
20837 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20838 exitg1 = 1;
20839 } else {
20840 b_kstr++;
20841 }
20842 } else {
20843 b_bool = true;
20844 exitg1 = 1;
20845 }
20846 } while (exitg1 == 0);
20847 }
20848
20849 if (b_bool) {
20850 b_kstr = 1;
20851 } else {
20852 b_kstr = -1;
20853 }
20854 }
20855
20856 cartesian_trajec_emxFree_char_T(&switch_expression);
20857 switch (b_kstr) {
20858 case 0:
20859 tmp[0] = 0;
20860 tmp[1] = 0;
20861 tmp[2] = 1;
20862 tmp[3] = 0;
20863 tmp[4] = 0;
20864 tmp[5] = 0;
20865 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20866 msubspace_data[b_kstr] = tmp[b_kstr];
20867 }
20868
20869 obj->JointInternal.PositionNumber = 1.0;
20870 obj->JointInternal.JointAxisInternal[0] = 0.0;
20871 obj->JointInternal.JointAxisInternal[1] = 0.0;
20872 obj->JointInternal.JointAxisInternal[2] = 1.0;
20873 break;
20874
20875 case 1:
20876 tmp[0] = 0;
20877 tmp[1] = 0;
20878 tmp[2] = 0;
20879 tmp[3] = 0;
20880 tmp[4] = 0;
20881 tmp[5] = 1;
20882 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20883 msubspace_data[b_kstr] = tmp[b_kstr];
20884 }
20885
20886 obj->JointInternal.PositionNumber = 1.0;
20887 obj->JointInternal.JointAxisInternal[0] = 0.0;
20888 obj->JointInternal.JointAxisInternal[1] = 0.0;
20889 obj->JointInternal.JointAxisInternal[2] = 1.0;
20890 break;
20891
20892 default:
20893 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20894 msubspace_data[b_kstr] = 0;
20895 }
20896
20897 obj->JointInternal.PositionNumber = 0.0;
20898 obj->JointInternal.JointAxisInternal[0] = 0.0;
20899 obj->JointInternal.JointAxisInternal[1] = 0.0;
20900 obj->JointInternal.JointAxisInternal[2] = 0.0;
20901 break;
20902 }
20903
20904 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20905 obj->JointInternal.MotionSubspace->size[1];
20906 obj->JointInternal.MotionSubspace->size[0] = 6;
20907 obj->JointInternal.MotionSubspace->size[1] = 1;
20908 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20909 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20910 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20911 }
20912
20913 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20914 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20915 }
20916
20917 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20918 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20919 }
20920
20921 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20922 obj->JointInternal.MotionSubspace->size[1];
20923 obj->JointInternal.MotionSubspace->size[0] = 6;
20924 obj->JointInternal.MotionSubspace->size[1] = 1;
20925 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20926 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20927 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20928 }
20929
20930 obj->JointInternal.JointAxisInternal[0] = 0.0;
20931 obj->JointInternal.JointAxisInternal[1] = 0.0;
20932 obj->JointInternal.JointAxisInternal[2] = 1.0;
20933 return b_obj;
20934}
20935
20936static n_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_astwhqf
20937 (n_robotics_manip_internal_Rig_T *obj)
20938{
20939 n_robotics_manip_internal_Rig_T *b_obj;
20940 int8_T msubspace_data[36];
20941 emxArray_char_T_cartesian_tra_T *switch_expression;
20942 boolean_T b_bool;
20943 int32_T b_kstr;
20944 char_T b[8];
20945 char_T b_0[9];
20946 int32_T loop_ub;
20947 int8_T tmp[6];
20948 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20949 'e', 'e' };
20950
20951 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
20952
20953 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20954
20955 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20956
20957 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20958 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20959
20960 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20961 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20962
20963 int32_T exitg1;
20964 b_obj = obj;
20965 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20966 obj->NameInternal->size[0] = 1;
20967 obj->NameInternal->size[1] = 11;
20968 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20969 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
20970 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20971 }
20972
20973 obj->ParentIndex = 7.0;
20974 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20975 obj->JointInternal.Type->size[0] = 1;
20976 obj->JointInternal.Type->size[1] = 5;
20977 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20978 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
20979 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20980 }
20981
20982 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20983 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20984 switch_expression->size[0] = 1;
20985 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20986 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20987 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20988 - 1;
20989 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20990 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20991 }
20992
20993 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20994 b[b_kstr] = tmp_2[b_kstr];
20995 }
20996
20997 b_bool = false;
20998 if (switch_expression->size[1] == 8) {
20999 b_kstr = 1;
21000 do {
21001 exitg1 = 0;
21002 if (b_kstr - 1 < 8) {
21003 loop_ub = b_kstr - 1;
21004 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21005 exitg1 = 1;
21006 } else {
21007 b_kstr++;
21008 }
21009 } else {
21010 b_bool = true;
21011 exitg1 = 1;
21012 }
21013 } while (exitg1 == 0);
21014 }
21015
21016 if (b_bool) {
21017 b_kstr = 0;
21018 } else {
21019 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21020 b_0[b_kstr] = tmp_3[b_kstr];
21021 }
21022
21023 b_bool = false;
21024 if (switch_expression->size[1] == 9) {
21025 b_kstr = 1;
21026 do {
21027 exitg1 = 0;
21028 if (b_kstr - 1 < 9) {
21029 loop_ub = b_kstr - 1;
21030 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21031 exitg1 = 1;
21032 } else {
21033 b_kstr++;
21034 }
21035 } else {
21036 b_bool = true;
21037 exitg1 = 1;
21038 }
21039 } while (exitg1 == 0);
21040 }
21041
21042 if (b_bool) {
21043 b_kstr = 1;
21044 } else {
21045 b_kstr = -1;
21046 }
21047 }
21048
21049 cartesian_trajec_emxFree_char_T(&switch_expression);
21050 switch (b_kstr) {
21051 case 0:
21052 tmp[0] = 0;
21053 tmp[1] = 0;
21054 tmp[2] = 1;
21055 tmp[3] = 0;
21056 tmp[4] = 0;
21057 tmp[5] = 0;
21058 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21059 msubspace_data[b_kstr] = tmp[b_kstr];
21060 }
21061
21062 obj->JointInternal.PositionNumber = 1.0;
21063 obj->JointInternal.JointAxisInternal[0] = 0.0;
21064 obj->JointInternal.JointAxisInternal[1] = 0.0;
21065 obj->JointInternal.JointAxisInternal[2] = 1.0;
21066 break;
21067
21068 case 1:
21069 tmp[0] = 0;
21070 tmp[1] = 0;
21071 tmp[2] = 0;
21072 tmp[3] = 0;
21073 tmp[4] = 0;
21074 tmp[5] = 1;
21075 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21076 msubspace_data[b_kstr] = tmp[b_kstr];
21077 }
21078
21079 obj->JointInternal.PositionNumber = 1.0;
21080 obj->JointInternal.JointAxisInternal[0] = 0.0;
21081 obj->JointInternal.JointAxisInternal[1] = 0.0;
21082 obj->JointInternal.JointAxisInternal[2] = 1.0;
21083 break;
21084
21085 default:
21086 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21087 msubspace_data[b_kstr] = 0;
21088 }
21089
21090 obj->JointInternal.PositionNumber = 0.0;
21091 obj->JointInternal.JointAxisInternal[0] = 0.0;
21092 obj->JointInternal.JointAxisInternal[1] = 0.0;
21093 obj->JointInternal.JointAxisInternal[2] = 0.0;
21094 break;
21095 }
21096
21097 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21098 obj->JointInternal.MotionSubspace->size[1];
21099 obj->JointInternal.MotionSubspace->size[0] = 6;
21100 obj->JointInternal.MotionSubspace->size[1] = 1;
21101 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21102 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21103 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21104 }
21105
21106 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21107 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
21108 }
21109
21110 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21111 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
21112 }
21113
21114 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21115 obj->JointInternal.MotionSubspace->size[1];
21116 obj->JointInternal.MotionSubspace->size[0] = 6;
21117 obj->JointInternal.MotionSubspace->size[1] = 1;
21118 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21119 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21120 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
21121 }
21122
21123 obj->JointInternal.JointAxisInternal[0] = 0.0;
21124 obj->JointInternal.JointAxisInternal[1] = 0.0;
21125 obj->JointInternal.JointAxisInternal[2] = 0.0;
21126 return b_obj;
21127}
21128
21129static o_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_astwhqf2
21130 (o_robotics_manip_internal_Rig_T *obj)
21131{
21132 o_robotics_manip_internal_Rig_T *b_obj;
21133 int8_T msubspace_data[36];
21134 emxArray_char_T_cartesian_tra_T *switch_expression;
21135 boolean_T b_bool;
21136 int32_T b_kstr;
21137 char_T b[8];
21138 char_T b_0[9];
21139 int32_T loop_ub;
21140 int8_T tmp[6];
21141 static const char_T tmp_0[5] = { 'w', 'o', 'r', 'l', 'd' };
21142
21143 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
21144
21145 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21146
21147 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21148
21149 int32_T exitg1;
21150 b_obj = obj;
21151 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21152 obj->NameInternal->size[0] = 1;
21153 obj->NameInternal->size[1] = 5;
21154 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21155 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
21156 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21157 }
21158
21159 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21160 obj->JointInternal.Type->size[0] = 1;
21161 obj->JointInternal.Type->size[1] = 5;
21162 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21163 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
21164 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21165 }
21166
21167 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
21168 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21169 switch_expression->size[0] = 1;
21170 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21171 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
21172 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21173 - 1;
21174 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21175 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21176 }
21177
21178 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21179 b[b_kstr] = tmp_2[b_kstr];
21180 }
21181
21182 b_bool = false;
21183 if (switch_expression->size[1] == 8) {
21184 b_kstr = 1;
21185 do {
21186 exitg1 = 0;
21187 if (b_kstr - 1 < 8) {
21188 loop_ub = b_kstr - 1;
21189 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21190 exitg1 = 1;
21191 } else {
21192 b_kstr++;
21193 }
21194 } else {
21195 b_bool = true;
21196 exitg1 = 1;
21197 }
21198 } while (exitg1 == 0);
21199 }
21200
21201 if (b_bool) {
21202 b_kstr = 0;
21203 } else {
21204 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21205 b_0[b_kstr] = tmp_3[b_kstr];
21206 }
21207
21208 b_bool = false;
21209 if (switch_expression->size[1] == 9) {
21210 b_kstr = 1;
21211 do {
21212 exitg1 = 0;
21213 if (b_kstr - 1 < 9) {
21214 loop_ub = b_kstr - 1;
21215 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21216 exitg1 = 1;
21217 } else {
21218 b_kstr++;
21219 }
21220 } else {
21221 b_bool = true;
21222 exitg1 = 1;
21223 }
21224 } while (exitg1 == 0);
21225 }
21226
21227 if (b_bool) {
21228 b_kstr = 1;
21229 } else {
21230 b_kstr = -1;
21231 }
21232 }
21233
21234 cartesian_trajec_emxFree_char_T(&switch_expression);
21235 switch (b_kstr) {
21236 case 0:
21237 tmp[0] = 0;
21238 tmp[1] = 0;
21239 tmp[2] = 1;
21240 tmp[3] = 0;
21241 tmp[4] = 0;
21242 tmp[5] = 0;
21243 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21244 msubspace_data[b_kstr] = tmp[b_kstr];
21245 }
21246
21247 obj->JointInternal.PositionNumber = 1.0;
21248 obj->JointInternal.JointAxisInternal[0] = 0.0;
21249 obj->JointInternal.JointAxisInternal[1] = 0.0;
21250 obj->JointInternal.JointAxisInternal[2] = 1.0;
21251 break;
21252
21253 case 1:
21254 tmp[0] = 0;
21255 tmp[1] = 0;
21256 tmp[2] = 0;
21257 tmp[3] = 0;
21258 tmp[4] = 0;
21259 tmp[5] = 1;
21260 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21261 msubspace_data[b_kstr] = tmp[b_kstr];
21262 }
21263
21264 obj->JointInternal.PositionNumber = 1.0;
21265 obj->JointInternal.JointAxisInternal[0] = 0.0;
21266 obj->JointInternal.JointAxisInternal[1] = 0.0;
21267 obj->JointInternal.JointAxisInternal[2] = 1.0;
21268 break;
21269
21270 default:
21271 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21272 msubspace_data[b_kstr] = 0;
21273 }
21274
21275 obj->JointInternal.PositionNumber = 0.0;
21276 obj->JointInternal.JointAxisInternal[0] = 0.0;
21277 obj->JointInternal.JointAxisInternal[1] = 0.0;
21278 obj->JointInternal.JointAxisInternal[2] = 0.0;
21279 break;
21280 }
21281
21282 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21283 obj->JointInternal.MotionSubspace->size[1];
21284 obj->JointInternal.MotionSubspace->size[0] = 6;
21285 obj->JointInternal.MotionSubspace->size[1] = 1;
21286 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21287 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21288 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21289 }
21290
21291 return b_obj;
21292}
21293
21294static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
21295 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
21296 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
21297 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
21298 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
21299 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
21300 n_robotics_manip_internal_Rig_T *iobj_7)
21301{
21302 p_robotics_manip_internal_Rig_T *b_obj;
21303 int32_T i;
21304 static const int8_T tmp[16] = { 0, 1, 2, 3, 4, 5, 6, 0, -1, 1, 2, 3, 4, 5, 6,
21305 -1 };
21306
21307 b_obj = obj;
21308 obj->Bodies[0] = cartesian_t_RigidBody_RigidBody(iobj_0);
21309 obj->Bodies[0]->Index = 1.0;
21310 obj->Bodies[1] = cartesian_RigidBody_RigidBody_a(iobj_7);
21311 obj->Bodies[1]->Index = 2.0;
21312 obj->Bodies[2] = cartesia_RigidBody_RigidBody_as(iobj_1);
21313 obj->Bodies[2]->Index = 3.0;
21314 obj->Bodies[3] = cartesi_RigidBody_RigidBody_ast(iobj_2);
21315 obj->Bodies[3]->Index = 4.0;
21316 obj->Bodies[4] = cartes_RigidBody_RigidBody_astw(iobj_3);
21317 obj->Bodies[4]->Index = 5.0;
21318 obj->Bodies[5] = carte_RigidBody_RigidBody_astwh(iobj_4);
21319 obj->Bodies[5]->Index = 6.0;
21320 obj->Bodies[6] = cart_RigidBody_RigidBody_astwhq(iobj_5);
21321 obj->Bodies[6]->Index = 7.0;
21322 obj->Bodies[7] = car_RigidBody_RigidBody_astwhqf(iobj_6);
21323 obj->Bodies[7]->Index = 8.0;
21324 obj->NumBodies = 8.0;
21325 obj->PositionNumber = 6.0;
21326 obj->VelocityNumber = 6.0;
21327 for (i = 0; i < 16; i++) {
21328 obj->PositionDoFMap[i] = tmp[i];
21329 }
21330
21331 ca_RigidBody_RigidBody_astwhqf2(&obj->Base);
21332 return b_obj;
21333}
21334
21335static void emxInitStruct_c_rigidBodyJoint2(c_rigidBodyJoint_cartesian__a_T
21336 *pStruct)
21337{
21338 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
21339}
21340
21341static void emxInitStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
21342 *pStruct)
21343{
21344 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
21345 emxInitStruct_c_rigidBodyJoint2(&pStruct->JointInternal);
21346}
21347
21348static void emxInitStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
21349 *pStruct)
21350{
21351 emxInitStruct_o_robotics_mani_a(&pStruct->Base);
21352}
21353
21354static void emxInitStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
21355 *pStruct)
21356{
21357 emxInitStruct_p_robotics_mani_a(&pStruct->TreeInternal);
21358}
21359
21360static void emxInitStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
21361 *pStruct)
21362{
21363 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
21364 emxInitStruct_c_rigidBodyJoint2(&pStruct->JointInternal);
21365}
21366
21367static n_robotics_manip_internal_R_a_T *c_RigidBody_RigidBody_astwhqf2a
21368 (n_robotics_manip_internal_R_a_T *obj)
21369{
21370 n_robotics_manip_internal_R_a_T *b_obj;
21371 emxArray_char_T_cartesian_tra_T *switch_expression;
21372 boolean_T b_bool;
21373 int32_T b_kstr;
21374 char_T b[8];
21375 char_T b_0[9];
21376 int32_T loop_ub;
21377 static const char_T tmp[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
21378 'l', 'i', 'n', 'k' };
21379
21380 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
21381
21382 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21383
21384 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21385
21386 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21387 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21388
21389 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21390 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21391
21392 int32_T exitg1;
21393 b_obj = obj;
21394 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21395 obj->NameInternal->size[0] = 1;
21396 obj->NameInternal->size[1] = 13;
21397 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21398 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
21399 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
21400 }
21401
21402 obj->ParentIndex = 0.0;
21403 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21404 obj->JointInternal.Type->size[0] = 1;
21405 obj->JointInternal.Type->size[1] = 5;
21406 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21407 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
21408 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
21409 }
21410
21411 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
21412 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21413 switch_expression->size[0] = 1;
21414 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21415 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
21416 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21417 - 1;
21418 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21419 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21420 }
21421
21422 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21423 b[b_kstr] = tmp_1[b_kstr];
21424 }
21425
21426 b_bool = false;
21427 if (switch_expression->size[1] == 8) {
21428 b_kstr = 1;
21429 do {
21430 exitg1 = 0;
21431 if (b_kstr - 1 < 8) {
21432 loop_ub = b_kstr - 1;
21433 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21434 exitg1 = 1;
21435 } else {
21436 b_kstr++;
21437 }
21438 } else {
21439 b_bool = true;
21440 exitg1 = 1;
21441 }
21442 } while (exitg1 == 0);
21443 }
21444
21445 if (b_bool) {
21446 b_kstr = 0;
21447 } else {
21448 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21449 b_0[b_kstr] = tmp_2[b_kstr];
21450 }
21451
21452 b_bool = false;
21453 if (switch_expression->size[1] == 9) {
21454 b_kstr = 1;
21455 do {
21456 exitg1 = 0;
21457 if (b_kstr - 1 < 9) {
21458 loop_ub = b_kstr - 1;
21459 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21460 exitg1 = 1;
21461 } else {
21462 b_kstr++;
21463 }
21464 } else {
21465 b_bool = true;
21466 exitg1 = 1;
21467 }
21468 } while (exitg1 == 0);
21469 }
21470
21471 if (b_bool) {
21472 b_kstr = 1;
21473 } else {
21474 b_kstr = -1;
21475 }
21476 }
21477
21478 cartesian_trajec_emxFree_char_T(&switch_expression);
21479 switch (b_kstr) {
21480 case 0:
21481 obj->JointInternal.PositionNumber = 1.0;
21482 obj->JointInternal.JointAxisInternal[0] = 0.0;
21483 obj->JointInternal.JointAxisInternal[1] = 0.0;
21484 obj->JointInternal.JointAxisInternal[2] = 1.0;
21485 break;
21486
21487 case 1:
21488 obj->JointInternal.PositionNumber = 1.0;
21489 obj->JointInternal.JointAxisInternal[0] = 0.0;
21490 obj->JointInternal.JointAxisInternal[1] = 0.0;
21491 obj->JointInternal.JointAxisInternal[2] = 1.0;
21492 break;
21493
21494 default:
21495 obj->JointInternal.PositionNumber = 0.0;
21496 obj->JointInternal.JointAxisInternal[0] = 0.0;
21497 obj->JointInternal.JointAxisInternal[1] = 0.0;
21498 obj->JointInternal.JointAxisInternal[2] = 0.0;
21499 break;
21500 }
21501
21502 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21503 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
21504 }
21505
21506 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21507 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
21508 }
21509
21510 obj->JointInternal.JointAxisInternal[0] = 0.0;
21511 obj->JointInternal.JointAxisInternal[1] = 0.0;
21512 obj->JointInternal.JointAxisInternal[2] = 0.0;
21513 return b_obj;
21514}
21515
21516static n_robotics_manip_internal_R_a_T *RigidBody_RigidBody_astwhqf2az
21517 (n_robotics_manip_internal_R_a_T *obj)
21518{
21519 n_robotics_manip_internal_R_a_T *b_obj;
21520 emxArray_char_T_cartesian_tra_T *switch_expression;
21521 boolean_T b_bool;
21522 int32_T b_kstr;
21523 char_T b[8];
21524 char_T b_0[9];
21525 int32_T loop_ub;
21526 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21527 '1' };
21528
21529 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21530
21531 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21532
21533 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21534 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
21535
21536 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21537 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21538
21539 int32_T exitg1;
21540 b_obj = obj;
21541 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21542 obj->NameInternal->size[0] = 1;
21543 obj->NameInternal->size[1] = 10;
21544 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21545 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21546 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
21547 }
21548
21549 obj->ParentIndex = 1.0;
21550 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21551 obj->JointInternal.Type->size[0] = 1;
21552 obj->JointInternal.Type->size[1] = 8;
21553 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21554 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21555 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
21556 }
21557
21558 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
21559 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21560 switch_expression->size[0] = 1;
21561 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21562 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
21563 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21564 - 1;
21565 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21566 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21567 }
21568
21569 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21570 b[b_kstr] = tmp_0[b_kstr];
21571 }
21572
21573 b_bool = false;
21574 if (switch_expression->size[1] == 8) {
21575 b_kstr = 1;
21576 do {
21577 exitg1 = 0;
21578 if (b_kstr - 1 < 8) {
21579 loop_ub = b_kstr - 1;
21580 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21581 exitg1 = 1;
21582 } else {
21583 b_kstr++;
21584 }
21585 } else {
21586 b_bool = true;
21587 exitg1 = 1;
21588 }
21589 } while (exitg1 == 0);
21590 }
21591
21592 if (b_bool) {
21593 b_kstr = 0;
21594 } else {
21595 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21596 b_0[b_kstr] = tmp_1[b_kstr];
21597 }
21598
21599 b_bool = false;
21600 if (switch_expression->size[1] == 9) {
21601 b_kstr = 1;
21602 do {
21603 exitg1 = 0;
21604 if (b_kstr - 1 < 9) {
21605 loop_ub = b_kstr - 1;
21606 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21607 exitg1 = 1;
21608 } else {
21609 b_kstr++;
21610 }
21611 } else {
21612 b_bool = true;
21613 exitg1 = 1;
21614 }
21615 } while (exitg1 == 0);
21616 }
21617
21618 if (b_bool) {
21619 b_kstr = 1;
21620 } else {
21621 b_kstr = -1;
21622 }
21623 }
21624
21625 cartesian_trajec_emxFree_char_T(&switch_expression);
21626 switch (b_kstr) {
21627 case 0:
21628 obj->JointInternal.PositionNumber = 1.0;
21629 obj->JointInternal.JointAxisInternal[0] = 0.0;
21630 obj->JointInternal.JointAxisInternal[1] = 0.0;
21631 obj->JointInternal.JointAxisInternal[2] = 1.0;
21632 break;
21633
21634 case 1:
21635 obj->JointInternal.PositionNumber = 1.0;
21636 obj->JointInternal.JointAxisInternal[0] = 0.0;
21637 obj->JointInternal.JointAxisInternal[1] = 0.0;
21638 obj->JointInternal.JointAxisInternal[2] = 1.0;
21639 break;
21640
21641 default:
21642 obj->JointInternal.PositionNumber = 0.0;
21643 obj->JointInternal.JointAxisInternal[0] = 0.0;
21644 obj->JointInternal.JointAxisInternal[1] = 0.0;
21645 obj->JointInternal.JointAxisInternal[2] = 0.0;
21646 break;
21647 }
21648
21649 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21650 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
21651 }
21652
21653 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21654 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
21655 }
21656
21657 obj->JointInternal.JointAxisInternal[0] = 0.0;
21658 obj->JointInternal.JointAxisInternal[1] = 0.0;
21659 obj->JointInternal.JointAxisInternal[2] = 1.0;
21660 return b_obj;
21661}
21662
21663static n_robotics_manip_internal_R_a_T *RigidBody_RigidBody_astwhqf2azt
21664 (n_robotics_manip_internal_R_a_T *obj)
21665{
21666 n_robotics_manip_internal_R_a_T *b_obj;
21667 emxArray_char_T_cartesian_tra_T *switch_expression;
21668 boolean_T b_bool;
21669 int32_T b_kstr;
21670 char_T b[8];
21671 char_T b_0[9];
21672 int32_T loop_ub;
21673 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21674 '2' };
21675
21676 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21677
21678 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21679
21680 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
21681 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
21682 0.0, 0.0, 0.0, 1.0 };
21683
21684 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21685 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21686
21687 int32_T exitg1;
21688 b_obj = obj;
21689 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21690 obj->NameInternal->size[0] = 1;
21691 obj->NameInternal->size[1] = 10;
21692 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21693 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21694 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
21695 }
21696
21697 obj->ParentIndex = 2.0;
21698 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21699 obj->JointInternal.Type->size[0] = 1;
21700 obj->JointInternal.Type->size[1] = 8;
21701 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21702 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21703 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
21704 }
21705
21706 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
21707 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21708 switch_expression->size[0] = 1;
21709 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21710 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
21711 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21712 - 1;
21713 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21714 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21715 }
21716
21717 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21718 b[b_kstr] = tmp_0[b_kstr];
21719 }
21720
21721 b_bool = false;
21722 if (switch_expression->size[1] == 8) {
21723 b_kstr = 1;
21724 do {
21725 exitg1 = 0;
21726 if (b_kstr - 1 < 8) {
21727 loop_ub = b_kstr - 1;
21728 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21729 exitg1 = 1;
21730 } else {
21731 b_kstr++;
21732 }
21733 } else {
21734 b_bool = true;
21735 exitg1 = 1;
21736 }
21737 } while (exitg1 == 0);
21738 }
21739
21740 if (b_bool) {
21741 b_kstr = 0;
21742 } else {
21743 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21744 b_0[b_kstr] = tmp_1[b_kstr];
21745 }
21746
21747 b_bool = false;
21748 if (switch_expression->size[1] == 9) {
21749 b_kstr = 1;
21750 do {
21751 exitg1 = 0;
21752 if (b_kstr - 1 < 9) {
21753 loop_ub = b_kstr - 1;
21754 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21755 exitg1 = 1;
21756 } else {
21757 b_kstr++;
21758 }
21759 } else {
21760 b_bool = true;
21761 exitg1 = 1;
21762 }
21763 } while (exitg1 == 0);
21764 }
21765
21766 if (b_bool) {
21767 b_kstr = 1;
21768 } else {
21769 b_kstr = -1;
21770 }
21771 }
21772
21773 cartesian_trajec_emxFree_char_T(&switch_expression);
21774 switch (b_kstr) {
21775 case 0:
21776 obj->JointInternal.PositionNumber = 1.0;
21777 obj->JointInternal.JointAxisInternal[0] = 0.0;
21778 obj->JointInternal.JointAxisInternal[1] = 0.0;
21779 obj->JointInternal.JointAxisInternal[2] = 1.0;
21780 break;
21781
21782 case 1:
21783 obj->JointInternal.PositionNumber = 1.0;
21784 obj->JointInternal.JointAxisInternal[0] = 0.0;
21785 obj->JointInternal.JointAxisInternal[1] = 0.0;
21786 obj->JointInternal.JointAxisInternal[2] = 1.0;
21787 break;
21788
21789 default:
21790 obj->JointInternal.PositionNumber = 0.0;
21791 obj->JointInternal.JointAxisInternal[0] = 0.0;
21792 obj->JointInternal.JointAxisInternal[1] = 0.0;
21793 obj->JointInternal.JointAxisInternal[2] = 0.0;
21794 break;
21795 }
21796
21797 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21798 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
21799 }
21800
21801 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21802 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
21803 }
21804
21805 obj->JointInternal.JointAxisInternal[0] = 0.0;
21806 obj->JointInternal.JointAxisInternal[1] = 0.0;
21807 obj->JointInternal.JointAxisInternal[2] = -1.0;
21808 return b_obj;
21809}
21810
21811static n_robotics_manip_internal_R_a_T *RigidBody_RigidBod_astwhqf2aztt
21812 (n_robotics_manip_internal_R_a_T *obj)
21813{
21814 n_robotics_manip_internal_R_a_T *b_obj;
21815 emxArray_char_T_cartesian_tra_T *switch_expression;
21816 boolean_T b_bool;
21817 int32_T b_kstr;
21818 char_T b[8];
21819 char_T b_0[9];
21820 int32_T loop_ub;
21821 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21822 '3' };
21823
21824 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21825
21826 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21827
21828 static const real_T tmp_2[16] = { 1.0, 2.0682310711021444E-13,
21829 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
21830 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
21831 1.0 };
21832
21833 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21834 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21835
21836 int32_T exitg1;
21837 b_obj = obj;
21838 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21839 obj->NameInternal->size[0] = 1;
21840 obj->NameInternal->size[1] = 10;
21841 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21842 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21843 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
21844 }
21845
21846 obj->ParentIndex = 3.0;
21847 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21848 obj->JointInternal.Type->size[0] = 1;
21849 obj->JointInternal.Type->size[1] = 8;
21850 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21851 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21852 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
21853 }
21854
21855 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
21856 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21857 switch_expression->size[0] = 1;
21858 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21859 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
21860 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21861 - 1;
21862 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21863 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21864 }
21865
21866 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21867 b[b_kstr] = tmp_0[b_kstr];
21868 }
21869
21870 b_bool = false;
21871 if (switch_expression->size[1] == 8) {
21872 b_kstr = 1;
21873 do {
21874 exitg1 = 0;
21875 if (b_kstr - 1 < 8) {
21876 loop_ub = b_kstr - 1;
21877 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21878 exitg1 = 1;
21879 } else {
21880 b_kstr++;
21881 }
21882 } else {
21883 b_bool = true;
21884 exitg1 = 1;
21885 }
21886 } while (exitg1 == 0);
21887 }
21888
21889 if (b_bool) {
21890 b_kstr = 0;
21891 } else {
21892 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21893 b_0[b_kstr] = tmp_1[b_kstr];
21894 }
21895
21896 b_bool = false;
21897 if (switch_expression->size[1] == 9) {
21898 b_kstr = 1;
21899 do {
21900 exitg1 = 0;
21901 if (b_kstr - 1 < 9) {
21902 loop_ub = b_kstr - 1;
21903 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21904 exitg1 = 1;
21905 } else {
21906 b_kstr++;
21907 }
21908 } else {
21909 b_bool = true;
21910 exitg1 = 1;
21911 }
21912 } while (exitg1 == 0);
21913 }
21914
21915 if (b_bool) {
21916 b_kstr = 1;
21917 } else {
21918 b_kstr = -1;
21919 }
21920 }
21921
21922 cartesian_trajec_emxFree_char_T(&switch_expression);
21923 switch (b_kstr) {
21924 case 0:
21925 obj->JointInternal.PositionNumber = 1.0;
21926 obj->JointInternal.JointAxisInternal[0] = 0.0;
21927 obj->JointInternal.JointAxisInternal[1] = 0.0;
21928 obj->JointInternal.JointAxisInternal[2] = 1.0;
21929 break;
21930
21931 case 1:
21932 obj->JointInternal.PositionNumber = 1.0;
21933 obj->JointInternal.JointAxisInternal[0] = 0.0;
21934 obj->JointInternal.JointAxisInternal[1] = 0.0;
21935 obj->JointInternal.JointAxisInternal[2] = 1.0;
21936 break;
21937
21938 default:
21939 obj->JointInternal.PositionNumber = 0.0;
21940 obj->JointInternal.JointAxisInternal[0] = 0.0;
21941 obj->JointInternal.JointAxisInternal[1] = 0.0;
21942 obj->JointInternal.JointAxisInternal[2] = 0.0;
21943 break;
21944 }
21945
21946 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21947 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
21948 }
21949
21950 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21951 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
21952 }
21953
21954 obj->JointInternal.JointAxisInternal[0] = 0.0;
21955 obj->JointInternal.JointAxisInternal[1] = 0.0;
21956 obj->JointInternal.JointAxisInternal[2] = 1.0;
21957 return b_obj;
21958}
21959
21960static n_robotics_manip_internal_R_a_T *RigidBody_RigidBo_astwhqf2azttx
21961 (n_robotics_manip_internal_R_a_T *obj)
21962{
21963 n_robotics_manip_internal_R_a_T *b_obj;
21964 emxArray_char_T_cartesian_tra_T *switch_expression;
21965 boolean_T b_bool;
21966 int32_T b_kstr;
21967 char_T b[8];
21968 char_T b_0[9];
21969 int32_T loop_ub;
21970 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21971 '4' };
21972
21973 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21974
21975 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21976
21977 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
21978 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
21979 0.0, -0.268, 0.0, 1.0 };
21980
21981 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21982 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21983
21984 int32_T exitg1;
21985 b_obj = obj;
21986 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21987 obj->NameInternal->size[0] = 1;
21988 obj->NameInternal->size[1] = 10;
21989 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
21990 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21991 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
21992 }
21993
21994 obj->ParentIndex = 4.0;
21995 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21996 obj->JointInternal.Type->size[0] = 1;
21997 obj->JointInternal.Type->size[1] = 8;
21998 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
21999 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22000 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22001 }
22002
22003 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
22004 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22005 switch_expression->size[0] = 1;
22006 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22007 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22008 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22009 - 1;
22010 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22011 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22012 }
22013
22014 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22015 b[b_kstr] = tmp_0[b_kstr];
22016 }
22017
22018 b_bool = false;
22019 if (switch_expression->size[1] == 8) {
22020 b_kstr = 1;
22021 do {
22022 exitg1 = 0;
22023 if (b_kstr - 1 < 8) {
22024 loop_ub = b_kstr - 1;
22025 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22026 exitg1 = 1;
22027 } else {
22028 b_kstr++;
22029 }
22030 } else {
22031 b_bool = true;
22032 exitg1 = 1;
22033 }
22034 } while (exitg1 == 0);
22035 }
22036
22037 if (b_bool) {
22038 b_kstr = 0;
22039 } else {
22040 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22041 b_0[b_kstr] = tmp_1[b_kstr];
22042 }
22043
22044 b_bool = false;
22045 if (switch_expression->size[1] == 9) {
22046 b_kstr = 1;
22047 do {
22048 exitg1 = 0;
22049 if (b_kstr - 1 < 9) {
22050 loop_ub = b_kstr - 1;
22051 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22052 exitg1 = 1;
22053 } else {
22054 b_kstr++;
22055 }
22056 } else {
22057 b_bool = true;
22058 exitg1 = 1;
22059 }
22060 } while (exitg1 == 0);
22061 }
22062
22063 if (b_bool) {
22064 b_kstr = 1;
22065 } else {
22066 b_kstr = -1;
22067 }
22068 }
22069
22070 cartesian_trajec_emxFree_char_T(&switch_expression);
22071 switch (b_kstr) {
22072 case 0:
22073 obj->JointInternal.PositionNumber = 1.0;
22074 obj->JointInternal.JointAxisInternal[0] = 0.0;
22075 obj->JointInternal.JointAxisInternal[1] = 0.0;
22076 obj->JointInternal.JointAxisInternal[2] = 1.0;
22077 break;
22078
22079 case 1:
22080 obj->JointInternal.PositionNumber = 1.0;
22081 obj->JointInternal.JointAxisInternal[0] = 0.0;
22082 obj->JointInternal.JointAxisInternal[1] = 0.0;
22083 obj->JointInternal.JointAxisInternal[2] = 1.0;
22084 break;
22085
22086 default:
22087 obj->JointInternal.PositionNumber = 0.0;
22088 obj->JointInternal.JointAxisInternal[0] = 0.0;
22089 obj->JointInternal.JointAxisInternal[1] = 0.0;
22090 obj->JointInternal.JointAxisInternal[2] = 0.0;
22091 break;
22092 }
22093
22094 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22095 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22096 }
22097
22098 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22099 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22100 }
22101
22102 obj->JointInternal.JointAxisInternal[0] = 0.0;
22103 obj->JointInternal.JointAxisInternal[1] = 0.0;
22104 obj->JointInternal.JointAxisInternal[2] = 1.0;
22105 return b_obj;
22106}
22107
22108static p_robotics_manip_internal_R_a_T *c_RigidBodyTree_RigidBodyTree_a
22109 (p_robotics_manip_internal_R_a_T *obj, n_robotics_manip_internal_R_a_T *iobj_0,
22110 n_robotics_manip_internal_R_a_T *iobj_1, n_robotics_manip_internal_R_a_T
22111 *iobj_2, n_robotics_manip_internal_R_a_T *iobj_3,
22112 n_robotics_manip_internal_R_a_T *iobj_4, n_robotics_manip_internal_R_a_T
22113 *iobj_5, n_robotics_manip_internal_R_a_T *iobj_6,
22114 n_robotics_manip_internal_R_a_T *iobj_7)
22115{
22116 p_robotics_manip_internal_R_a_T *b_obj;
22117 o_robotics_manip_internal_R_a_T *obj_0;
22118 emxArray_char_T_cartesian_tra_T *switch_expression;
22119 boolean_T b_bool;
22120 int32_T b_kstr;
22121 char_T b[8];
22122 char_T b_0[9];
22123 int32_T loop_ub;
22124 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22125 '5' };
22126
22127 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22128
22129 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22130
22131 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22132 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22133
22134 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22135 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22136
22137 static const char_T tmp_4[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22138 '6' };
22139
22140 static const real_T tmp_5[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22141 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
22142
22143 static const char_T tmp_6[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22144 'e', 'e' };
22145
22146 static const char_T tmp_7[5] = { 'f', 'i', 'x', 'e', 'd' };
22147
22148 static const char_T tmp_8[5] = { 'w', 'o', 'r', 'l', 'd' };
22149
22150 int32_T exitg1;
22151 b_obj = obj;
22152 obj->Bodies[0] = c_RigidBody_RigidBody_astwhqf2a(iobj_0);
22153 obj->Bodies[1] = RigidBody_RigidBody_astwhqf2az(iobj_7);
22154 obj->Bodies[2] = RigidBody_RigidBody_astwhqf2azt(iobj_1);
22155 obj->Bodies[3] = RigidBody_RigidBod_astwhqf2aztt(iobj_2);
22156 obj->Bodies[4] = RigidBody_RigidBo_astwhqf2azttx(iobj_3);
22157 b_kstr = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size[1];
22158 iobj_4->NameInternal->size[0] = 1;
22159 iobj_4->NameInternal->size[1] = 10;
22160 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, b_kstr);
22161 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22162 iobj_4->NameInternal->data[b_kstr] = tmp[b_kstr];
22163 }
22164
22165 iobj_4->ParentIndex = 5.0;
22166 b_kstr = iobj_4->JointInternal.Type->size[0] * iobj_4->
22167 JointInternal.Type->size[1];
22168 iobj_4->JointInternal.Type->size[0] = 1;
22169 iobj_4->JointInternal.Type->size[1] = 8;
22170 cartes_emxEnsureCapacity_char_T(iobj_4->JointInternal.Type, b_kstr);
22171 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22172 iobj_4->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22173 }
22174
22175 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
22176 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22177 switch_expression->size[0] = 1;
22178 switch_expression->size[1] = iobj_4->JointInternal.Type->size[1];
22179 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22180 loop_ub = iobj_4->JointInternal.Type->size[0] * iobj_4->
22181 JointInternal.Type->size[1] - 1;
22182 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22183 switch_expression->data[b_kstr] = iobj_4->JointInternal.Type->data[b_kstr];
22184 }
22185
22186 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22187 b[b_kstr] = tmp_0[b_kstr];
22188 }
22189
22190 b_bool = false;
22191 if (switch_expression->size[1] == 8) {
22192 b_kstr = 1;
22193 do {
22194 exitg1 = 0;
22195 if (b_kstr - 1 < 8) {
22196 loop_ub = b_kstr - 1;
22197 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22198 exitg1 = 1;
22199 } else {
22200 b_kstr++;
22201 }
22202 } else {
22203 b_bool = true;
22204 exitg1 = 1;
22205 }
22206 } while (exitg1 == 0);
22207 }
22208
22209 if (b_bool) {
22210 b_kstr = 0;
22211 } else {
22212 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22213 b_0[b_kstr] = tmp_1[b_kstr];
22214 }
22215
22216 b_bool = false;
22217 if (switch_expression->size[1] == 9) {
22218 b_kstr = 1;
22219 do {
22220 exitg1 = 0;
22221 if (b_kstr - 1 < 9) {
22222 loop_ub = b_kstr - 1;
22223 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22224 exitg1 = 1;
22225 } else {
22226 b_kstr++;
22227 }
22228 } else {
22229 b_bool = true;
22230 exitg1 = 1;
22231 }
22232 } while (exitg1 == 0);
22233 }
22234
22235 if (b_bool) {
22236 b_kstr = 1;
22237 } else {
22238 b_kstr = -1;
22239 }
22240 }
22241
22242 switch (b_kstr) {
22243 case 0:
22244 iobj_4->JointInternal.PositionNumber = 1.0;
22245 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
22246 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
22247 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
22248 break;
22249
22250 case 1:
22251 iobj_4->JointInternal.PositionNumber = 1.0;
22252 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
22253 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
22254 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
22255 break;
22256
22257 default:
22258 iobj_4->JointInternal.PositionNumber = 0.0;
22259 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
22260 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
22261 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
22262 break;
22263 }
22264
22265 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22266 iobj_4->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22267 }
22268
22269 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22270 iobj_4->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22271 }
22272
22273 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
22274 iobj_4->JointInternal.JointAxisInternal[1] = 1.0;
22275 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
22276 obj->Bodies[5] = iobj_4;
22277 b_kstr = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size[1];
22278 iobj_5->NameInternal->size[0] = 1;
22279 iobj_5->NameInternal->size[1] = 10;
22280 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, b_kstr);
22281 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22282 iobj_5->NameInternal->data[b_kstr] = tmp_4[b_kstr];
22283 }
22284
22285 iobj_5->ParentIndex = 6.0;
22286 b_kstr = iobj_5->JointInternal.Type->size[0] * iobj_5->
22287 JointInternal.Type->size[1];
22288 iobj_5->JointInternal.Type->size[0] = 1;
22289 iobj_5->JointInternal.Type->size[1] = 8;
22290 cartes_emxEnsureCapacity_char_T(iobj_5->JointInternal.Type, b_kstr);
22291 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22292 iobj_5->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22293 }
22294
22295 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22296 switch_expression->size[0] = 1;
22297 switch_expression->size[1] = iobj_5->JointInternal.Type->size[1];
22298 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22299 loop_ub = iobj_5->JointInternal.Type->size[0] * iobj_5->
22300 JointInternal.Type->size[1] - 1;
22301 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22302 switch_expression->data[b_kstr] = iobj_5->JointInternal.Type->data[b_kstr];
22303 }
22304
22305 b_bool = false;
22306 if (switch_expression->size[1] == 8) {
22307 b_kstr = 1;
22308 do {
22309 exitg1 = 0;
22310 if (b_kstr - 1 < 8) {
22311 loop_ub = b_kstr - 1;
22312 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22313 exitg1 = 1;
22314 } else {
22315 b_kstr++;
22316 }
22317 } else {
22318 b_bool = true;
22319 exitg1 = 1;
22320 }
22321 } while (exitg1 == 0);
22322 }
22323
22324 if (b_bool) {
22325 b_kstr = 0;
22326 } else {
22327 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22328 b_0[b_kstr] = tmp_1[b_kstr];
22329 }
22330
22331 b_bool = false;
22332 if (switch_expression->size[1] == 9) {
22333 b_kstr = 1;
22334 do {
22335 exitg1 = 0;
22336 if (b_kstr - 1 < 9) {
22337 loop_ub = b_kstr - 1;
22338 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22339 exitg1 = 1;
22340 } else {
22341 b_kstr++;
22342 }
22343 } else {
22344 b_bool = true;
22345 exitg1 = 1;
22346 }
22347 } while (exitg1 == 0);
22348 }
22349
22350 if (b_bool) {
22351 b_kstr = 1;
22352 } else {
22353 b_kstr = -1;
22354 }
22355 }
22356
22357 switch (b_kstr) {
22358 case 0:
22359 iobj_5->JointInternal.PositionNumber = 1.0;
22360 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
22361 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
22362 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
22363 break;
22364
22365 case 1:
22366 iobj_5->JointInternal.PositionNumber = 1.0;
22367 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
22368 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
22369 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
22370 break;
22371
22372 default:
22373 iobj_5->JointInternal.PositionNumber = 0.0;
22374 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
22375 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
22376 iobj_5->JointInternal.JointAxisInternal[2] = 0.0;
22377 break;
22378 }
22379
22380 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22381 iobj_5->JointInternal.JointToParentTransform[b_kstr] = tmp_5[b_kstr];
22382 }
22383
22384 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22385 iobj_5->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22386 }
22387
22388 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
22389 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
22390 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
22391 obj->Bodies[6] = iobj_5;
22392 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
22393 iobj_6->NameInternal->size[0] = 1;
22394 iobj_6->NameInternal->size[1] = 11;
22395 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
22396 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
22397 iobj_6->NameInternal->data[b_kstr] = tmp_6[b_kstr];
22398 }
22399
22400 iobj_6->ParentIndex = 7.0;
22401 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
22402 JointInternal.Type->size[1];
22403 iobj_6->JointInternal.Type->size[0] = 1;
22404 iobj_6->JointInternal.Type->size[1] = 5;
22405 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
22406 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22407 iobj_6->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
22408 }
22409
22410 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22411 switch_expression->size[0] = 1;
22412 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
22413 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22414 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
22415 JointInternal.Type->size[1] - 1;
22416 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22417 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
22418 }
22419
22420 b_bool = false;
22421 if (switch_expression->size[1] == 8) {
22422 b_kstr = 1;
22423 do {
22424 exitg1 = 0;
22425 if (b_kstr - 1 < 8) {
22426 loop_ub = b_kstr - 1;
22427 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22428 exitg1 = 1;
22429 } else {
22430 b_kstr++;
22431 }
22432 } else {
22433 b_bool = true;
22434 exitg1 = 1;
22435 }
22436 } while (exitg1 == 0);
22437 }
22438
22439 if (b_bool) {
22440 b_kstr = 0;
22441 } else {
22442 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22443 b_0[b_kstr] = tmp_1[b_kstr];
22444 }
22445
22446 b_bool = false;
22447 if (switch_expression->size[1] == 9) {
22448 b_kstr = 1;
22449 do {
22450 exitg1 = 0;
22451 if (b_kstr - 1 < 9) {
22452 loop_ub = b_kstr - 1;
22453 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22454 exitg1 = 1;
22455 } else {
22456 b_kstr++;
22457 }
22458 } else {
22459 b_bool = true;
22460 exitg1 = 1;
22461 }
22462 } while (exitg1 == 0);
22463 }
22464
22465 if (b_bool) {
22466 b_kstr = 1;
22467 } else {
22468 b_kstr = -1;
22469 }
22470 }
22471
22472 switch (b_kstr) {
22473 case 0:
22474 iobj_6->JointInternal.PositionNumber = 1.0;
22475 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
22476 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
22477 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
22478 break;
22479
22480 case 1:
22481 iobj_6->JointInternal.PositionNumber = 1.0;
22482 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
22483 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
22484 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
22485 break;
22486
22487 default:
22488 iobj_6->JointInternal.PositionNumber = 0.0;
22489 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
22490 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
22491 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
22492 break;
22493 }
22494
22495 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22496 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22497 }
22498
22499 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22500 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22501 }
22502
22503 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
22504 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
22505 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
22506 obj->Bodies[7] = iobj_6;
22507 obj->NumBodies = 8.0;
22508 obj->PositionNumber = 6.0;
22509 obj_0 = &obj->Base;
22510 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
22511 obj->Base.NameInternal->size[0] = 1;
22512 obj->Base.NameInternal->size[1] = 5;
22513 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
22514 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22515 obj->Base.NameInternal->data[b_kstr] = tmp_8[b_kstr];
22516 }
22517
22518 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
22519 Base.JointInternal.Type->size[1];
22520 obj->Base.JointInternal.Type->size[0] = 1;
22521 obj->Base.JointInternal.Type->size[1] = 5;
22522 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
22523 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22524 obj_0->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
22525 }
22526
22527 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22528 switch_expression->size[0] = 1;
22529 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
22530 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22531 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
22532 Base.JointInternal.Type->size[1] - 1;
22533 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22534 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
22535 }
22536
22537 b_bool = false;
22538 if (switch_expression->size[1] == 8) {
22539 b_kstr = 1;
22540 do {
22541 exitg1 = 0;
22542 if (b_kstr - 1 < 8) {
22543 loop_ub = b_kstr - 1;
22544 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22545 exitg1 = 1;
22546 } else {
22547 b_kstr++;
22548 }
22549 } else {
22550 b_bool = true;
22551 exitg1 = 1;
22552 }
22553 } while (exitg1 == 0);
22554 }
22555
22556 if (b_bool) {
22557 b_kstr = 0;
22558 } else {
22559 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22560 b_0[b_kstr] = tmp_1[b_kstr];
22561 }
22562
22563 b_bool = false;
22564 if (switch_expression->size[1] == 9) {
22565 b_kstr = 1;
22566 do {
22567 exitg1 = 0;
22568 if (b_kstr - 1 < 9) {
22569 loop_ub = b_kstr - 1;
22570 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22571 exitg1 = 1;
22572 } else {
22573 b_kstr++;
22574 }
22575 } else {
22576 b_bool = true;
22577 exitg1 = 1;
22578 }
22579 } while (exitg1 == 0);
22580 }
22581
22582 if (b_bool) {
22583 b_kstr = 1;
22584 } else {
22585 b_kstr = -1;
22586 }
22587 }
22588
22589 cartesian_trajec_emxFree_char_T(&switch_expression);
22590 switch (b_kstr) {
22591 case 0:
22592 obj->Base.JointInternal.PositionNumber = 1.0;
22593 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
22594 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
22595 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
22596 break;
22597
22598 case 1:
22599 obj->Base.JointInternal.PositionNumber = 1.0;
22600 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
22601 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
22602 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
22603 break;
22604
22605 default:
22606 obj->Base.JointInternal.PositionNumber = 0.0;
22607 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
22608 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
22609 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
22610 break;
22611 }
22612
22613 return b_obj;
22614}
22615
22616// Model step function
22617void cartesian_trajectory_planner_step(void)
22618{
22619 robotics_slmanip_internal__as_T *obj;
22620 emxArray_real_T_cartesian_tra_T *b;
22621 emxArray_f_cell_wrap_cartesia_T *obj_f;
22622
22623 // Outputs for Atomic SubSystem: '<Root>/Subscribe'
22624 // MATLABSystem: '<S9>/SourceBlock' incorporates:
22625 // Inport: '<S13>/In1'
22626
22627 cartesian_trajectory_planner_B.b_varargout_1 =
22628 Sub_cartesian_trajectory_planner_280.getLatestMessage
22629 (&cartesian_trajectory_planner_B.b_varargout_2);
22630
22631 // Outputs for Enabled SubSystem: '<S9>/Enabled Subsystem' incorporates:
22632 // EnablePort: '<S13>/Enable'
22633
22634 if (cartesian_trajectory_planner_B.b_varargout_1) {
22635 cartesian_trajectory_planner_B.In1 =
22636 cartesian_trajectory_planner_B.b_varargout_2;
22637 }
22638
22639 // End of MATLABSystem: '<S9>/SourceBlock'
22640 // End of Outputs for SubSystem: '<S9>/Enabled Subsystem'
22641 // End of Outputs for SubSystem: '<Root>/Subscribe'
22642
22643 // MATLAB Function: '<Root>/MATLAB Function1'
22644 cartesian_trajectory_planner_B.time =
22645 cartesian_trajectory_planner_B.In1.Clock_.Nsec / 1.0E+9 +
22646 cartesian_trajectory_planner_B.In1.Clock_.Sec;
22647
22648 // MATLABSystem: '<Root>/Get Parameter'
22649 ParamGet_cartesian_trajectory_planner_271.get_parameter
22650 (&cartesian_trajectory_planner_B.rtb_signal2_idx_1);
22651
22652 // MATLABSystem: '<Root>/Get Parameter1'
22653 ParamGet_cartesian_trajectory_planner_272.get_parameter
22654 (&cartesian_trajectory_planner_B.rtb_signal2_idx_2);
22655
22656 // MATLABSystem: '<S14>/Get Parameter3'
22657 ParamGet_cartesian_trajectory_planner_291.get_parameter
22658 (&cartesian_trajectory_planner_B.tc);
22659
22660 // MATLABSystem: '<S14>/Get Parameter4'
22661 ParamGet_cartesian_trajectory_planner_292.get_parameter
22662 (&cartesian_trajectory_planner_B.SpMax);
22663
22664 // MATLABSystem: '<S14>/Get Parameter5'
22665 ParamGet_cartesian_trajectory_planner_293.get_parameter
22666 (&cartesian_trajectory_planner_B.maxval);
22667
22668 // MATLABSystem: '<S14>/Get Parameter6'
22669 ParamGet_cartesian_trajectory_planner_294.get_parameter
22670 (&cartesian_trajectory_planner_B.s_i);
22671
22672 // MATLABSystem: '<S14>/Get Parameter'
22673 ParamGet_cartesian_trajectory_planner_288.get_parameter
22674 (&cartesian_trajectory_planner_B.value);
22675
22676 // MATLABSystem: '<S14>/Get Parameter1'
22677 ParamGet_cartesian_trajectory_planner_289.get_parameter
22678 (&cartesian_trajectory_planner_B.value_h);
22679
22680 // MATLABSystem: '<S14>/Get Parameter2'
22681 ParamGet_cartesian_trajectory_planner_290.get_parameter
22682 (&cartesian_trajectory_planner_B.value_n);
22683
22684 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
22685 // MATLABSystem: '<S14>/Get Parameter'
22686 // MATLABSystem: '<S14>/Get Parameter1'
22687 // MATLABSystem: '<S14>/Get Parameter2'
22688 // MATLABSystem: '<S14>/Get Parameter3'
22689 // MATLABSystem: '<S14>/Get Parameter4'
22690 // MATLABSystem: '<S14>/Get Parameter5'
22691 // MATLABSystem: '<S14>/Get Parameter6'
22692
22693 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
22694 (((cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc +
22695 cartesian_trajectory_planner_B.SpMax *
22696 cartesian_trajectory_planner_B.SpMax) +
22697 cartesian_trajectory_planner_B.maxval *
22698 cartesian_trajectory_planner_B.maxval) +
22699 cartesian_trajectory_planner_B.s_i * cartesian_trajectory_planner_B.s_i);
22700 cartesian_trajectory_planner_B.tc *= cartesian_trajectory_planner_B.s_vel;
22701 cartesian_trajectory_planner_B.SpMax *= cartesian_trajectory_planner_B.s_vel;
22702 cartesian_trajectory_planner_B.maxval *= cartesian_trajectory_planner_B.s_vel;
22703 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.s_i *
22704 cartesian_trajectory_planner_B.s_vel;
22705 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tft *
22706 cartesian_trajectory_planner_B.tft;
22707 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.maxval *
22708 cartesian_trajectory_planner_B.maxval;
22709 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
22710 (cartesian_trajectory_planner_B.s_i + cartesian_trajectory_planner_B.s_vel) *
22711 2.0;
22712 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax *
22713 cartesian_trajectory_planner_B.maxval;
22714 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22715 cartesian_trajectory_planner_B.tft;
22716 cartesian_trajectory_planner_B.tempR[1] =
22717 (cartesian_trajectory_planner_B.tcstar -
22718 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22719 cartesian_trajectory_planner_B.teta_vel = cartesian_trajectory_planner_B.SpMax
22720 * cartesian_trajectory_planner_B.tft;
22721 cartesian_trajectory_planner_B.tf = cartesian_trajectory_planner_B.tc *
22722 cartesian_trajectory_planner_B.maxval;
22723 cartesian_trajectory_planner_B.tempR[2] =
22724 (cartesian_trajectory_planner_B.teta_vel + cartesian_trajectory_planner_B.tf)
22725 * 2.0;
22726 cartesian_trajectory_planner_B.tempR[3] =
22727 (cartesian_trajectory_planner_B.tcstar +
22728 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22729 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax *
22730 cartesian_trajectory_planner_B.SpMax;
22731 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
22732 (cartesian_trajectory_planner_B.tcstar +
22733 cartesian_trajectory_planner_B.s_vel) * 2.0;
22734 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.maxval *
22735 cartesian_trajectory_planner_B.tft;
22736 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22737 cartesian_trajectory_planner_B.SpMax;
22738 cartesian_trajectory_planner_B.tempR[5] =
22739 (cartesian_trajectory_planner_B.s_vel -
22740 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22741 cartesian_trajectory_planner_B.tempR[6] =
22742 (cartesian_trajectory_planner_B.teta_vel - cartesian_trajectory_planner_B.tf)
22743 * 2.0;
22744 cartesian_trajectory_planner_B.tempR[7] =
22745 (cartesian_trajectory_planner_B.s_vel +
22746 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22747 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
22748 (cartesian_trajectory_planner_B.tcstar + cartesian_trajectory_planner_B.s_i)
22749 * 2.0;
22750 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
22751 3; cartesian_trajectory_planner_B.r1++) {
22752 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22753 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 - 1] =
22754 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22755 1) * 3];
22756 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22757 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 2] =
22758 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22759 1) * 3 + 1];
22760 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22761 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 5] =
22762 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22763 1) * 3 + 2];
22764 }
22765
22766 memset(&cartesian_trajectory_planner_B.out[0], 0, sizeof(real_T) << 4U);
22767 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
22768 3; cartesian_trajectory_planner_B.r1++) {
22769 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
22770 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2] =
22771 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1];
22772 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 1] =
22773 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 + 1];
22774 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 2] =
22775 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 + 2];
22776 }
22777
22778 cartesian_trajectory_planner_B.out[15] = 1.0;
22779 cartesian_trajectory_planner_B.out[12] = cartesian_trajectory_planner_B.value;
22780 cartesian_trajectory_planner_B.out[13] =
22781 cartesian_trajectory_planner_B.value_h;
22782 cartesian_trajectory_planner_B.out[14] =
22783 cartesian_trajectory_planner_B.value_n;
22784
22785 // MATLABSystem: '<S15>/Get Parameter3'
22786 ParamGet_cartesian_trajectory_planner_303.get_parameter
22787 (&cartesian_trajectory_planner_B.tc);
22788
22789 // MATLABSystem: '<S15>/Get Parameter4'
22790 ParamGet_cartesian_trajectory_planner_304.get_parameter
22791 (&cartesian_trajectory_planner_B.SpMax);
22792
22793 // MATLABSystem: '<S15>/Get Parameter5'
22794 ParamGet_cartesian_trajectory_planner_305.get_parameter
22795 (&cartesian_trajectory_planner_B.maxval);
22796
22797 // MATLABSystem: '<S15>/Get Parameter6'
22798 ParamGet_cartesian_trajectory_planner_306.get_parameter
22799 (&cartesian_trajectory_planner_B.s_i);
22800
22801 // MATLABSystem: '<S15>/Get Parameter'
22802 ParamGet_cartesian_trajectory_planner_300.get_parameter
22803 (&cartesian_trajectory_planner_B.value_o);
22804
22805 // MATLABSystem: '<S15>/Get Parameter1'
22806 ParamGet_cartesian_trajectory_planner_301.get_parameter
22807 (&cartesian_trajectory_planner_B.value_c);
22808
22809 // MATLABSystem: '<S15>/Get Parameter2'
22810 ParamGet_cartesian_trajectory_planner_302.get_parameter
22811 (&cartesian_trajectory_planner_B.value_b);
22812
22813 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1' incorporates:
22814 // MATLABSystem: '<S15>/Get Parameter'
22815 // MATLABSystem: '<S15>/Get Parameter1'
22816 // MATLABSystem: '<S15>/Get Parameter2'
22817 // MATLABSystem: '<S15>/Get Parameter3'
22818 // MATLABSystem: '<S15>/Get Parameter4'
22819 // MATLABSystem: '<S15>/Get Parameter5'
22820 // MATLABSystem: '<S15>/Get Parameter6'
22821
22822 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
22823 (((cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc +
22824 cartesian_trajectory_planner_B.SpMax *
22825 cartesian_trajectory_planner_B.SpMax) +
22826 cartesian_trajectory_planner_B.maxval *
22827 cartesian_trajectory_planner_B.maxval) +
22828 cartesian_trajectory_planner_B.s_i * cartesian_trajectory_planner_B.s_i);
22829 cartesian_trajectory_planner_B.tc *= cartesian_trajectory_planner_B.s_vel;
22830 cartesian_trajectory_planner_B.SpMax *= cartesian_trajectory_planner_B.s_vel;
22831 cartesian_trajectory_planner_B.maxval *= cartesian_trajectory_planner_B.s_vel;
22832 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.s_i *
22833 cartesian_trajectory_planner_B.s_vel;
22834 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tft *
22835 cartesian_trajectory_planner_B.tft;
22836 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.maxval *
22837 cartesian_trajectory_planner_B.maxval;
22838 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
22839 (cartesian_trajectory_planner_B.s_i + cartesian_trajectory_planner_B.s_vel) *
22840 2.0;
22841 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax *
22842 cartesian_trajectory_planner_B.maxval;
22843 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22844 cartesian_trajectory_planner_B.tft;
22845 cartesian_trajectory_planner_B.tempR[1] =
22846 (cartesian_trajectory_planner_B.tcstar -
22847 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22848 cartesian_trajectory_planner_B.teta_vel = cartesian_trajectory_planner_B.SpMax
22849 * cartesian_trajectory_planner_B.tft;
22850 cartesian_trajectory_planner_B.tf = cartesian_trajectory_planner_B.tc *
22851 cartesian_trajectory_planner_B.maxval;
22852 cartesian_trajectory_planner_B.tempR[2] =
22853 (cartesian_trajectory_planner_B.teta_vel + cartesian_trajectory_planner_B.tf)
22854 * 2.0;
22855 cartesian_trajectory_planner_B.tempR[3] =
22856 (cartesian_trajectory_planner_B.tcstar +
22857 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22858 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax *
22859 cartesian_trajectory_planner_B.SpMax;
22860 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
22861 (cartesian_trajectory_planner_B.tcstar +
22862 cartesian_trajectory_planner_B.s_vel) * 2.0;
22863 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.maxval *
22864 cartesian_trajectory_planner_B.tft;
22865 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22866 cartesian_trajectory_planner_B.SpMax;
22867 cartesian_trajectory_planner_B.tempR[5] =
22868 (cartesian_trajectory_planner_B.s_vel -
22869 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22870 cartesian_trajectory_planner_B.tempR[6] =
22871 (cartesian_trajectory_planner_B.teta_vel - cartesian_trajectory_planner_B.tf)
22872 * 2.0;
22873 cartesian_trajectory_planner_B.tempR[7] =
22874 (cartesian_trajectory_planner_B.s_vel +
22875 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22876 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
22877 (cartesian_trajectory_planner_B.tcstar + cartesian_trajectory_planner_B.s_i)
22878 * 2.0;
22879 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
22880 3; cartesian_trajectory_planner_B.r1++) {
22881 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22882 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 - 1] =
22883 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22884 1) * 3];
22885 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22886 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 2] =
22887 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22888 1) * 3 + 1];
22889 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22890 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 5] =
22891 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2 -
22892 1) * 3 + 2];
22893 }
22894
22895 memset(&cartesian_trajectory_planner_B.out_j[0], 0, sizeof(real_T) << 4U);
22896 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
22897 3; cartesian_trajectory_planner_B.r1++) {
22898 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
22899 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2] =
22900 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1];
22901 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2 + 1] =
22902 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 + 1];
22903 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2 + 2] =
22904 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 + 2];
22905 }
22906
22907 cartesian_trajectory_planner_B.out_j[15] = 1.0;
22908 cartesian_trajectory_planner_B.out_j[12] =
22909 cartesian_trajectory_planner_B.value_o;
22910 cartesian_trajectory_planner_B.out_j[13] =
22911 cartesian_trajectory_planner_B.value_c;
22912 cartesian_trajectory_planner_B.out_j[14] =
22913 cartesian_trajectory_planner_B.value_b;
22914
22915 // MATLAB Function: '<Root>/Traslazione ' incorporates:
22916 // MATLABSystem: '<Root>/Get Parameter'
22917 // MATLABSystem: '<Root>/Get Parameter1'
22918
22919 cartesian_trajectory_planner_B.tc =
22920 cartesian_trajectory_planner_B.rtb_signal2_idx_1 /
22921 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
22922 cartesian_trajectory_planner_B.SpMax = cartesian_trajectory_planner_B.tc *
22923 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
22924
22925 // MATLAB Function: '<Root>/Completa' incorporates:
22926 // MATLAB Function: '<Root>/Traslazione '
22927
22928 memset(&cartesian_trajectory_planner_B.POSA[0], 0, sizeof(real_T) << 4U);
22929
22930 // MATLAB Function: '<Root>/Traslazione ' incorporates:
22931 // MATLAB Function: '<Root>/Completa'
22932 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
22933 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
22934 // MATLABSystem: '<Root>/Get Parameter'
22935 // MATLABSystem: '<Root>/Get Parameter1'
22936 // MATLABSystem: '<S14>/Get Parameter'
22937 // MATLABSystem: '<S14>/Get Parameter1'
22938 // MATLABSystem: '<S14>/Get Parameter2'
22939 // MATLABSystem: '<S15>/Get Parameter'
22940 // MATLABSystem: '<S15>/Get Parameter1'
22941 // MATLABSystem: '<S15>/Get Parameter2'
22942
22943 cartesian_trajectory_planner_B.s_i = 0.0;
22944 cartesian_trajectory_planner_B.s_vel = 0.0;
22945 cartesian_trajectory_planner_B.tetaFIN =
22946 cartesian_trajectory_planner_B.value_o -
22947 cartesian_trajectory_planner_B.value;
22948 cartesian_trajectory_planner_B.out_m[0] =
22949 cartesian_trajectory_planner_B.tetaFIN;
22950 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.value_c -
22951 cartesian_trajectory_planner_B.value_h;
22952 cartesian_trajectory_planner_B.out_m[1] = cartesian_trajectory_planner_B.tft;
22953 cartesian_trajectory_planner_B.teta_vel =
22954 cartesian_trajectory_planner_B.value_b -
22955 cartesian_trajectory_planner_B.value_n;
22956 cartesian_trajectory_planner_B.out_m[2] =
22957 cartesian_trajectory_planner_B.teta_vel;
22958 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_pla_norm_l
22959 (cartesian_trajectory_planner_B.out_m);
22960 if (cartesian_trajectory_planner_B.SpMax >=
22961 cartesian_trajectory_planner_B.maxval) {
22962 cartesian_trajectory_planner_B.tcstar = sqrt
22963 (cartesian_trajectory_planner_B.maxval /
22964 cartesian_trajectory_planner_B.rtb_signal2_idx_2);
22965 cartesian_trajectory_planner_B.tf = 2.0 *
22966 cartesian_trajectory_planner_B.tcstar;
22967 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
22968 (cartesian_trajectory_planner_B.time <
22969 cartesian_trajectory_planner_B.tcstar)) {
22970 cartesian_trajectory_planner_B.s_i = 0.5 *
22971 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
22972 cartesian_trajectory_planner_B.time *
22973 cartesian_trajectory_planner_B.time;
22974 cartesian_trajectory_planner_B.s_vel =
22975 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
22976 cartesian_trajectory_planner_B.time;
22977 }
22978
22979 if ((cartesian_trajectory_planner_B.time >=
22980 cartesian_trajectory_planner_B.tcstar) &&
22981 (cartesian_trajectory_planner_B.time <=
22982 cartesian_trajectory_planner_B.tf)) {
22983 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tf -
22984 cartesian_trajectory_planner_B.time;
22985 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.maxval
22986 - 0.5 * cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
22987 cartesian_trajectory_planner_B.s_vel *
22988 cartesian_trajectory_planner_B.s_vel;
22989 cartesian_trajectory_planner_B.s_vel *=
22990 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
22991 }
22992
22993 if (cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tf)
22994 {
22995 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.maxval;
22996 cartesian_trajectory_planner_B.s_vel = 0.0;
22997 }
22998 }
22999
23000 if (cartesian_trajectory_planner_B.SpMax <
23001 cartesian_trajectory_planner_B.maxval) {
23002 cartesian_trajectory_planner_B.tcstar =
23003 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23004 cartesian_trajectory_planner_B.tc;
23005 cartesian_trajectory_planner_B.tf = (cartesian_trajectory_planner_B.maxval -
23006 cartesian_trajectory_planner_B.tcstar * cartesian_trajectory_planner_B.tc)
23007 * (1.0 / cartesian_trajectory_planner_B.rtb_signal2_idx_1) + 2.0 *
23008 cartesian_trajectory_planner_B.tc;
23009 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23010 (cartesian_trajectory_planner_B.time <=
23011 cartesian_trajectory_planner_B.tc)) {
23012 cartesian_trajectory_planner_B.s_i = 0.5 *
23013 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23014 cartesian_trajectory_planner_B.time *
23015 cartesian_trajectory_planner_B.time;
23016 cartesian_trajectory_planner_B.s_vel =
23017 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23018 cartesian_trajectory_planner_B.time;
23019 }
23020
23021 cartesian_trajectory_planner_B.d = cartesian_trajectory_planner_B.tf -
23022 cartesian_trajectory_planner_B.tc;
23023 if ((cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tc)
23024 && (cartesian_trajectory_planner_B.time <=
23025 cartesian_trajectory_planner_B.d)) {
23026 cartesian_trajectory_planner_B.s_i = (cartesian_trajectory_planner_B.time
23027 - cartesian_trajectory_planner_B.tc / 2.0) *
23028 cartesian_trajectory_planner_B.tcstar;
23029 cartesian_trajectory_planner_B.s_vel =
23030 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23031 }
23032
23033 if ((cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.d)
23034 && (cartesian_trajectory_planner_B.time <=
23035 cartesian_trajectory_planner_B.tf)) {
23036 cartesian_trajectory_planner_B.out_m[0] =
23037 cartesian_trajectory_planner_B.tetaFIN;
23038 cartesian_trajectory_planner_B.out_m[1] =
23039 cartesian_trajectory_planner_B.tft;
23040 cartesian_trajectory_planner_B.out_m[2] =
23041 cartesian_trajectory_planner_B.teta_vel;
23042 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_pla_norm_l
23043 (cartesian_trajectory_planner_B.out_m) - 0.5 *
23044 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23045 (cartesian_trajectory_planner_B.tf - cartesian_trajectory_planner_B.time)
23046 * (cartesian_trajectory_planner_B.tf -
23047 cartesian_trajectory_planner_B.time);
23048 cartesian_trajectory_planner_B.s_vel = (cartesian_trajectory_planner_B.tf
23049 - cartesian_trajectory_planner_B.time) *
23050 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
23051 }
23052
23053 if (cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tf)
23054 {
23055 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.maxval;
23056 cartesian_trajectory_planner_B.s_vel = 0.0;
23057 }
23058 }
23059
23060 cartesian_trajectory_planner_B.P_e[0] = cartesian_trajectory_planner_B.s_i /
23061 cartesian_trajectory_planner_B.maxval *
23062 cartesian_trajectory_planner_B.tetaFIN +
23063 cartesian_trajectory_planner_B.value;
23064 cartesian_trajectory_planner_B.out_m[0] =
23065 cartesian_trajectory_planner_B.tetaFIN;
23066 cartesian_trajectory_planner_B.out_m0[0] =
23067 cartesian_trajectory_planner_B.tetaFIN;
23068 cartesian_trajectory_planner_B.out_m[1] = cartesian_trajectory_planner_B.tft;
23069 cartesian_trajectory_planner_B.out_m0[1] = cartesian_trajectory_planner_B.tft;
23070 cartesian_trajectory_planner_B.out_m[2] =
23071 cartesian_trajectory_planner_B.teta_vel;
23072 cartesian_trajectory_planner_B.out_m0[2] =
23073 cartesian_trajectory_planner_B.teta_vel;
23074 cartesian_trajectory_planner_B.P_e[1] = cartesian_trajectory_planner_B.s_i /
23075 cartesian_trajectory_pla_norm_l(cartesian_trajectory_planner_B.out_m) *
23076 cartesian_trajectory_planner_B.tft + cartesian_trajectory_planner_B.value_h;
23077 cartesian_trajectory_planner_B.P_e[2] = cartesian_trajectory_planner_B.s_i /
23078 cartesian_trajectory_pla_norm_l(cartesian_trajectory_planner_B.out_m0) *
23079 cartesian_trajectory_planner_B.teta_vel +
23080 cartesian_trajectory_planner_B.value_n;
23081 cartesian_trajectory_planner_B.POSA[15] = 1.0;
23082 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
23083 3; cartesian_trajectory_planner_B.r1++) {
23084 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r1 + 12] =
23085 cartesian_trajectory_planner_B.P_e[cartesian_trajectory_planner_B.r1];
23086 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
23087 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1] =
23088 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2];
23089 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1 +
23090 1] =
23091 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2 + 1];
23092 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1 +
23093 2] =
23094 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2 + 2];
23095 }
23096
23097 cartesian_trajectory_planner_B.r1 = 0;
23098 cartesian_trajectory_planner_B.r2 = 1;
23099 cartesian_trajectory_planner_B.r3 = 2;
23100 cartesian_trajectory_planner_B.maxval = fabs
23101 (cartesian_trajectory_planner_B.out_j[0]);
23102 cartesian_trajectory_planner_B.s_i = fabs
23103 (cartesian_trajectory_planner_B.out_j[1]);
23104 if (cartesian_trajectory_planner_B.s_i > cartesian_trajectory_planner_B.maxval)
23105 {
23106 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_planner_B.s_i;
23107 cartesian_trajectory_planner_B.r1 = 1;
23108 cartesian_trajectory_planner_B.r2 = 0;
23109 }
23110
23111 if (fabs(cartesian_trajectory_planner_B.out_j[2]) >
23112 cartesian_trajectory_planner_B.maxval) {
23113 cartesian_trajectory_planner_B.r1 = 2;
23114 cartesian_trajectory_planner_B.r2 = 1;
23115 cartesian_trajectory_planner_B.r3 = 0;
23116 }
23117
23118 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2] =
23119 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r2] /
23120 cartesian_trajectory_planner_B.out_j[cartesian_trajectory_planner_B.r1];
23121 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3] /=
23122 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23123 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3] -=
23124 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3] *
23125 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23126 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3] -=
23127 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3] *
23128 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23129 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 6] -=
23130 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6] *
23131 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23132 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6] -=
23133 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6] *
23134 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23135 if (fabs
23136 (cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 +
23137 3]) > fabs
23138 (cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 +
23139 3])) {
23140 cartesian_trajectory_planner_B.rtemp = cartesian_trajectory_planner_B.r2;
23141 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r3;
23142 cartesian_trajectory_planner_B.r3 = cartesian_trajectory_planner_B.rtemp;
23143 }
23144
23145 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3] /=
23146 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3];
23147 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6] -=
23148 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3] *
23149 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 6];
23150 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1] =
23151 cartesian_trajectory_planner_B.out[0] /
23152 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23153 cartesian_trajectory_planner_B.maxval =
23154 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3];
23155 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2] =
23156 cartesian_trajectory_planner_B.out[4] -
23157 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1]
23158 * cartesian_trajectory_planner_B.maxval;
23159 cartesian_trajectory_planner_B.s_i =
23160 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6];
23161 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3] =
23162 cartesian_trajectory_planner_B.out[8] -
23163 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1]
23164 * cartesian_trajectory_planner_B.s_i;
23165 cartesian_trajectory_planner_B.tcstar =
23166 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3];
23167 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2] /=
23168 cartesian_trajectory_planner_B.tcstar;
23169 cartesian_trajectory_planner_B.tetaFIN =
23170 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 6];
23171 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3] -=
23172 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2]
23173 * cartesian_trajectory_planner_B.tetaFIN;
23174 cartesian_trajectory_planner_B.tft =
23175 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6];
23176 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3] /=
23177 cartesian_trajectory_planner_B.tft;
23178 cartesian_trajectory_planner_B.teta_vel =
23179 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3];
23180 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2] -=
23181 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3]
23182 * cartesian_trajectory_planner_B.teta_vel;
23183 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1] -=
23184 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3]
23185 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23186 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1] -=
23187 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2]
23188 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23189 cartesian_trajectory_planner_B.rtemp = 3 * cartesian_trajectory_planner_B.r1 +
23190 1;
23191 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] =
23192 cartesian_trajectory_planner_B.out[1] /
23193 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23194 cartesian_trajectory_planner_B.Matrot_tmp = 3 *
23195 cartesian_trajectory_planner_B.r2 + 1;
23196 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23197 = cartesian_trajectory_planner_B.out[5] -
23198 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] *
23199 cartesian_trajectory_planner_B.maxval;
23200 cartesian_trajectory_planner_B.Matrot_tmp_f = 3 *
23201 cartesian_trajectory_planner_B.r3 + 1;
23202 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23203 = cartesian_trajectory_planner_B.out[9] -
23204 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] *
23205 cartesian_trajectory_planner_B.s_i;
23206 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23207 /= cartesian_trajectory_planner_B.tcstar;
23208 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23209 -=
23210 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23211 * cartesian_trajectory_planner_B.tetaFIN;
23212 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23213 /= cartesian_trajectory_planner_B.tft;
23214 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23215 -=
23216 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23217 * cartesian_trajectory_planner_B.teta_vel;
23218 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23219 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23220 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23221 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23222 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23223 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23224 cartesian_trajectory_planner_B.rtemp = 3 * cartesian_trajectory_planner_B.r1 +
23225 2;
23226 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] =
23227 cartesian_trajectory_planner_B.out[2] /
23228 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23229 cartesian_trajectory_planner_B.Matrot_tmp = 3 *
23230 cartesian_trajectory_planner_B.r2 + 2;
23231 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23232 = cartesian_trajectory_planner_B.out[6] -
23233 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] *
23234 cartesian_trajectory_planner_B.maxval;
23235 cartesian_trajectory_planner_B.Matrot_tmp_f = 3 *
23236 cartesian_trajectory_planner_B.r3 + 2;
23237 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23238 = cartesian_trajectory_planner_B.out[10] -
23239 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] *
23240 cartesian_trajectory_planner_B.s_i;
23241 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23242 /= cartesian_trajectory_planner_B.tcstar;
23243 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23244 -=
23245 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23246 * cartesian_trajectory_planner_B.tetaFIN;
23247 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23248 /= cartesian_trajectory_planner_B.tft;
23249 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23250 -=
23251 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23252 * cartesian_trajectory_planner_B.teta_vel;
23253 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23254 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_f]
23255 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23256 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23257 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23258 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23259 cartesian_trajectory_planner_B.tetaFIN = acos
23260 ((((cartesian_trajectory_planner_B.Matrot[0] +
23261 cartesian_trajectory_planner_B.Matrot[4]) +
23262 cartesian_trajectory_planner_B.Matrot[8]) - 1.0) / 2.0);
23263 cartesian_trajectory_planner_B.maxval = 1.0 / (2.0 * sin
23264 (cartesian_trajectory_planner_B.tetaFIN));
23265 cartesian_trajectory_planner_B.P_e[0] =
23266 (cartesian_trajectory_planner_B.Matrot[5] -
23267 cartesian_trajectory_planner_B.Matrot[7]) *
23268 cartesian_trajectory_planner_B.maxval;
23269 cartesian_trajectory_planner_B.P_e[1] =
23270 (cartesian_trajectory_planner_B.Matrot[6] -
23271 cartesian_trajectory_planner_B.Matrot[2]) *
23272 cartesian_trajectory_planner_B.maxval;
23273 cartesian_trajectory_planner_B.P_e[2] =
23274 (cartesian_trajectory_planner_B.Matrot[1] -
23275 cartesian_trajectory_planner_B.Matrot[3]) *
23276 cartesian_trajectory_planner_B.maxval;
23277 cartesian_trajectory_planner_B.s_i = 0.0;
23278 cartesian_trajectory_planner_B.teta_vel = 0.0;
23279 if (cartesian_trajectory_planner_B.SpMax >=
23280 cartesian_trajectory_planner_B.tetaFIN) {
23281 cartesian_trajectory_planner_B.tcstar = sqrt
23282 (cartesian_trajectory_planner_B.tetaFIN /
23283 cartesian_trajectory_planner_B.rtb_signal2_idx_2);
23284 cartesian_trajectory_planner_B.tft = 2.0 *
23285 cartesian_trajectory_planner_B.tcstar;
23286 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23287 (cartesian_trajectory_planner_B.time <
23288 cartesian_trajectory_planner_B.tcstar)) {
23289 cartesian_trajectory_planner_B.s_i = 0.5 *
23290 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23291 cartesian_trajectory_planner_B.time *
23292 cartesian_trajectory_planner_B.time;
23293 cartesian_trajectory_planner_B.teta_vel =
23294 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23295 cartesian_trajectory_planner_B.time;
23296 }
23297
23298 if ((cartesian_trajectory_planner_B.time >=
23299 cartesian_trajectory_planner_B.tcstar) &&
23300 (cartesian_trajectory_planner_B.time <=
23301 cartesian_trajectory_planner_B.tft)) {
23302 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_planner_B.tft
23303 - cartesian_trajectory_planner_B.time;
23304 cartesian_trajectory_planner_B.s_i =
23305 cartesian_trajectory_planner_B.tetaFIN - 0.5 *
23306 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23307 cartesian_trajectory_planner_B.maxval *
23308 cartesian_trajectory_planner_B.maxval;
23309 cartesian_trajectory_planner_B.teta_vel =
23310 cartesian_trajectory_planner_B.maxval *
23311 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
23312 }
23313
23314 if (cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tft)
23315 {
23316 cartesian_trajectory_planner_B.s_i =
23317 cartesian_trajectory_planner_B.tetaFIN;
23318 cartesian_trajectory_planner_B.teta_vel = 0.0;
23319 }
23320 }
23321
23322 if (cartesian_trajectory_planner_B.SpMax <
23323 cartesian_trajectory_planner_B.tetaFIN) {
23324 cartesian_trajectory_planner_B.tft = (cartesian_trajectory_planner_B.tetaFIN
23325 - cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23326 cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc) *
23327 (1.0 / cartesian_trajectory_planner_B.rtb_signal2_idx_1) + 2.0 *
23328 cartesian_trajectory_planner_B.tc;
23329 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23330 (cartesian_trajectory_planner_B.time <=
23331 cartesian_trajectory_planner_B.tc)) {
23332 cartesian_trajectory_planner_B.s_i = 0.5 *
23333 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23334 cartesian_trajectory_planner_B.time *
23335 cartesian_trajectory_planner_B.time;
23336 cartesian_trajectory_planner_B.teta_vel =
23337 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23338 cartesian_trajectory_planner_B.time;
23339 }
23340
23341 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_planner_B.tft -
23342 cartesian_trajectory_planner_B.tc;
23343 if ((cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tc)
23344 && (cartesian_trajectory_planner_B.time <=
23345 cartesian_trajectory_planner_B.maxval)) {
23346 cartesian_trajectory_planner_B.s_i = (cartesian_trajectory_planner_B.time
23347 - cartesian_trajectory_planner_B.tc / 2.0) *
23348 (cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23349 cartesian_trajectory_planner_B.tc);
23350 cartesian_trajectory_planner_B.teta_vel =
23351 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23352 }
23353
23354 if ((cartesian_trajectory_planner_B.time >
23355 cartesian_trajectory_planner_B.maxval) &&
23356 (cartesian_trajectory_planner_B.time <=
23357 cartesian_trajectory_planner_B.tft)) {
23358 cartesian_trajectory_planner_B.s_i =
23359 cartesian_trajectory_planner_B.tetaFIN - 0.5 *
23360 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23361 (cartesian_trajectory_planner_B.tft -
23362 cartesian_trajectory_planner_B.time) *
23363 (cartesian_trajectory_planner_B.tft -
23364 cartesian_trajectory_planner_B.time);
23365 cartesian_trajectory_planner_B.teta_vel =
23366 (cartesian_trajectory_planner_B.tft -
23367 cartesian_trajectory_planner_B.time) *
23368 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23369 }
23370
23371 if (cartesian_trajectory_planner_B.time > cartesian_trajectory_planner_B.tft)
23372 {
23373 cartesian_trajectory_planner_B.s_i =
23374 cartesian_trajectory_planner_B.tetaFIN;
23375 cartesian_trajectory_planner_B.teta_vel = 0.0;
23376 }
23377 }
23378
23379 cartesian_trajectory_planner_B.Vel[0] = (cartesian_trajectory_planner_B.value
23380 - cartesian_trajectory_planner_B.value_o) *
23381 cartesian_trajectory_planner_B.s_vel;
23382 cartesian_trajectory_planner_B.Vel[3] =
23383 cartesian_trajectory_planner_B.teta_vel *
23384 cartesian_trajectory_planner_B.P_e[0];
23385 cartesian_trajectory_planner_B.Vel[1] =
23386 (cartesian_trajectory_planner_B.value_h -
23387 cartesian_trajectory_planner_B.value_c) *
23388 cartesian_trajectory_planner_B.s_vel;
23389 cartesian_trajectory_planner_B.Vel[4] =
23390 cartesian_trajectory_planner_B.teta_vel *
23391 cartesian_trajectory_planner_B.P_e[1];
23392 cartesian_trajectory_planner_B.Vel[2] =
23393 (cartesian_trajectory_planner_B.value_n -
23394 cartesian_trajectory_planner_B.value_b) *
23395 cartesian_trajectory_planner_B.s_vel;
23396 cartesian_trajectory_planner_B.Vel[5] =
23397 cartesian_trajectory_planner_B.teta_vel *
23398 cartesian_trajectory_planner_B.P_e[2];
23399 cartesian_trajectory_planner_B.time = cartesian_trajectory_planner_B.P_e[0];
23400 cartesian_trajectory_planner_B.rtb_signal2_idx_1 =
23401 cartesian_trajectory_planner_B.P_e[1];
23402 cartesian_trajectory_planner_B.rtb_signal2_idx_2 =
23403 cartesian_trajectory_planner_B.P_e[2];
23404
23405 // MATLABSystem: '<Root>/Coordinate Transformation Conversion2' incorporates:
23406 // MATLAB Function: '<Root>/Traslazione '
23407
23408 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
23409 ((cartesian_trajectory_planner_B.time * cartesian_trajectory_planner_B.time
23410 + cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23411 cartesian_trajectory_planner_B.rtb_signal2_idx_1) +
23412 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23413 cartesian_trajectory_planner_B.rtb_signal2_idx_2);
23414 cartesian_trajectory_planner_B.P_e[0] = cartesian_trajectory_planner_B.time *
23415 cartesian_trajectory_planner_B.s_vel;
23416 cartesian_trajectory_planner_B.P_e[1] =
23417 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23418 cartesian_trajectory_planner_B.s_vel;
23419 cartesian_trajectory_planner_B.P_e[2] =
23420 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23421 cartesian_trajectory_planner_B.s_vel;
23422 cartesian_trajectory_planner_B.time = cos(cartesian_trajectory_planner_B.s_i);
23423 cartesian_trajectory_planner_B.rtb_signal2_idx_1 = sin
23424 (cartesian_trajectory_planner_B.s_i);
23425 cartesian_trajectory_planner_B.tempR[0] = cartesian_trajectory_planner_B.P_e[0]
23426 * cartesian_trajectory_planner_B.P_e[0] * (1.0 -
23427 cartesian_trajectory_planner_B.time) + cartesian_trajectory_planner_B.time;
23428 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.P_e[1] *
23429 cartesian_trajectory_planner_B.P_e[0] * (1.0 -
23430 cartesian_trajectory_planner_B.time);
23431 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.P_e[2] *
23432 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23433 cartesian_trajectory_planner_B.tempR[1] = cartesian_trajectory_planner_B.s_vel
23434 - cartesian_trajectory_planner_B.s_i;
23435 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.P_e[2] *
23436 cartesian_trajectory_planner_B.P_e[0] * (1.0 -
23437 cartesian_trajectory_planner_B.time);
23438 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.P_e[1]
23439 * cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23440 cartesian_trajectory_planner_B.tempR[2] =
23441 cartesian_trajectory_planner_B.tcstar +
23442 cartesian_trajectory_planner_B.tetaFIN;
23443 cartesian_trajectory_planner_B.tempR[3] = cartesian_trajectory_planner_B.s_vel
23444 + cartesian_trajectory_planner_B.s_i;
23445 cartesian_trajectory_planner_B.tempR[4] = cartesian_trajectory_planner_B.P_e[1]
23446 * cartesian_trajectory_planner_B.P_e[1] * (1.0 -
23447 cartesian_trajectory_planner_B.time) + cartesian_trajectory_planner_B.time;
23448 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.P_e[2] *
23449 cartesian_trajectory_planner_B.P_e[1] * (1.0 -
23450 cartesian_trajectory_planner_B.time);
23451 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.P_e[0] *
23452 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23453 cartesian_trajectory_planner_B.tempR[5] = cartesian_trajectory_planner_B.s_vel
23454 - cartesian_trajectory_planner_B.s_i;
23455 cartesian_trajectory_planner_B.tempR[6] =
23456 cartesian_trajectory_planner_B.tcstar -
23457 cartesian_trajectory_planner_B.tetaFIN;
23458 cartesian_trajectory_planner_B.tempR[7] = cartesian_trajectory_planner_B.s_vel
23459 + cartesian_trajectory_planner_B.s_i;
23460 cartesian_trajectory_planner_B.tempR[8] = cartesian_trajectory_planner_B.P_e[2]
23461 * cartesian_trajectory_planner_B.P_e[2] * (1.0 -
23462 cartesian_trajectory_planner_B.time) + cartesian_trajectory_planner_B.time;
23463 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
23464 3; cartesian_trajectory_planner_B.r1++) {
23465 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23466 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 - 1]
23467 = cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23468 - 1) * 3];
23469 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23470 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 + 2]
23471 = cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23472 - 1) * 3 + 1];
23473 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23474 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 + 5]
23475 = cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23476 - 1) * 3 + 2];
23477 }
23478
23479 // MATLAB Function: '<Root>/Completa' incorporates:
23480 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
23481 // MATLABSystem: '<Root>/Coordinate Transformation Conversion2'
23482
23483 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
23484 3; cartesian_trajectory_planner_B.r1++) {
23485 for (cartesian_trajectory_planner_B.r2 = 0;
23486 cartesian_trajectory_planner_B.r2 < 3;
23487 cartesian_trajectory_planner_B.r2++) {
23488 cartesian_trajectory_planner_B.r3 = cartesian_trajectory_planner_B.r2 +
23489 (cartesian_trajectory_planner_B.r1 << 2);
23490 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] =
23491 0.0;
23492 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23493 cartesian_trajectory_planner_B.Matrot[3 *
23494 cartesian_trajectory_planner_B.r1] *
23495 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2];
23496 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23497 cartesian_trajectory_planner_B.Matrot[3 *
23498 cartesian_trajectory_planner_B.r1 + 1] *
23499 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 4];
23500 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23501 cartesian_trajectory_planner_B.Matrot[3 *
23502 cartesian_trajectory_planner_B.r1 + 2] *
23503 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 8];
23504 }
23505 }
23506
23507 // MATLABSystem: '<S5>/MATLAB System' incorporates:
23508 // MATLABSystem: '<S11>/Get Parameter'
23509 // MATLABSystem: '<S11>/Get Parameter1'
23510 // MATLABSystem: '<S11>/Get Parameter2'
23511 // MATLABSystem: '<S11>/Get Parameter3'
23512 // MATLABSystem: '<S11>/Get Parameter4'
23513 // MATLABSystem: '<S11>/Get Parameter5'
23514
23515 ParamGet_cartesian_trajectory_planner_312.get_parameter
23516 (&cartesian_trajectory_planner_B.MatrixMultiply1[0]);
23517 ParamGet_cartesian_trajectory_planner_313.get_parameter
23518 (&cartesian_trajectory_planner_B.MatrixMultiply1[1]);
23519 ParamGet_cartesian_trajectory_planner_314.get_parameter
23520 (&cartesian_trajectory_planner_B.MatrixMultiply1[2]);
23521 ParamGet_cartesian_trajectory_planner_315.get_parameter
23522 (&cartesian_trajectory_planner_B.MatrixMultiply1[3]);
23523 ParamGet_cartesian_trajectory_planner_316.get_parameter
23524 (&cartesian_trajectory_planner_B.MatrixMultiply1[4]);
23525 ParamGet_cartesian_trajectory_planner_317.get_parameter
23526 (&cartesian_trajectory_planner_B.MatrixMultiply1[5]);
23527 obj = &cartesian_trajectory_planner_DW.obj;
23528 if (cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized != 1) {
23529 cartesian_trajectory_planner_DW.obj.IKInternal.isSetupComplete = false;
23530 cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized = 1;
23531 car_inverseKinematics_setupImpl
23532 (&cartesian_trajectory_planner_DW.obj.IKInternal,
23533 &cartesian_trajectory_planner_DW.gobj_85);
23534 obj->IKInternal.isSetupComplete = true;
23535 }
23536
23537 cartesian_trajec_emxInit_real_T(&b, 2);
23538 cartesian_t_emxInit_f_cell_wrap(&obj_f, 2);
23539
23540 // Delay: '<Root>/Delay'
23541 for (cartesian_trajectory_planner_B.r1 = 0; cartesian_trajectory_planner_B.r1 <
23542 6; cartesian_trajectory_planner_B.r1++) {
23543 cartesian_trajectory_planner_B.dv1[cartesian_trajectory_planner_B.r1] =
23544 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.r1];
23545 }
23546
23547 // MATLABSystem: '<S5>/MATLAB System' incorporates:
23548 // Delay: '<Root>/Delay'
23549
23550 cart_inverseKinematics_stepImpl(&obj->IKInternal,
23551 cartesian_trajectory_planner_B.POSA,
23552 cartesian_trajectory_planner_B.MatrixMultiply1,
23553 cartesian_trajectory_planner_B.dv1,
23554 cartesian_trajectory_planner_DW.Delay_DSTATE);
23555
23556 // MATLABSystem: '<S3>/MATLAB System' incorporates:
23557 // Delay: '<Root>/Delay'
23558
23559 RigidBodyTree_geometricJacobian
23560 (&cartesian_trajectory_planner_DW.obj_g.TreeInternal,
23561 cartesian_trajectory_planner_DW.Delay_DSTATE, b);
23562
23563 // MATLABSystem: '<S4>/MATLAB System' incorporates:
23564 // Delay: '<Root>/Delay'
23565
23566 RigidBodyTree_forwardKinemati_a
23567 (&cartesian_trajectory_planner_DW.obj_f.TreeInternal,
23568 cartesian_trajectory_planner_DW.Delay_DSTATE, obj_f);
23569
23570 // Product: '<Root>/Reciprocal' incorporates:
23571 // MATLABSystem: '<S3>/MATLAB System'
23572
23573 rt_invd6x6_snf(b->data, cartesian_trajectory_planner_B.dv);
23574 cartesian_t_emxFree_f_cell_wrap(&obj_f);
23575 cartesian_trajec_emxFree_real_T(&b);
23576
23577 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
23578 // Constant: '<S1>/Constant'
23579
23580 cartesian_trajectory_planner_B.msg =
23581 cartesian_trajectory_planner_P.Constant_Value;
23582 cartesian_trajectory_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;
23583 cartesian_trajectory_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;
23584 for (cartesian_trajectory_planner_B.r2 = 0; cartesian_trajectory_planner_B.r2 <
23585 6; cartesian_trajectory_planner_B.r2++) {
23586 // Product: '<Root>/MatrixMultiply1' incorporates:
23587 // Product: '<Root>/Reciprocal'
23588
23589 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2]
23590 = 0.0;
23591 for (cartesian_trajectory_planner_B.r1 = 0;
23592 cartesian_trajectory_planner_B.r1 < 6;
23593 cartesian_trajectory_planner_B.r1++) {
23594 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2]
23595 += cartesian_trajectory_planner_B.dv[6 *
23596 cartesian_trajectory_planner_B.r1 + cartesian_trajectory_planner_B.r2] *
23597 cartesian_trajectory_planner_B.Vel[cartesian_trajectory_planner_B.r1];
23598 }
23599
23600 // End of Product: '<Root>/MatrixMultiply1'
23601
23602 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
23603 // Delay: '<Root>/Delay'
23604
23605 cartesian_trajectory_planner_B.msg.Velocities[cartesian_trajectory_planner_B.r2]
23606 =
23607 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2];
23608 cartesian_trajectory_planner_B.msg.Positions[cartesian_trajectory_planner_B.r2]
23609 =
23610 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.r2];
23611 }
23612
23613 // Outputs for Atomic SubSystem: '<Root>/Publish'
23614 // MATLABSystem: '<S8>/SinkBlock'
23615 Pub_cartesian_trajectory_planner_278.publish
23616 (&cartesian_trajectory_planner_B.msg);
23617
23618 // End of Outputs for SubSystem: '<Root>/Publish'
23619
23620 // MATLABSystem: '<S10>/Get Parameter'
23621 ParamGet_cartesian_trajectory_planner_283.get_parameter
23622 (&cartesian_trajectory_planner_B.rtb_signal2_idx_1);
23623
23624 // MATLABSystem: '<S10>/Get Parameter1'
23625 ParamGet_cartesian_trajectory_planner_284.get_parameter
23626 (&cartesian_trajectory_planner_B.rtb_signal2_idx_1);
23627}
23628
23629// Model initialize function
23630void cartesian_trajectory_planner_initialize(void)
23631{
23632 // Registration code
23633
23634 // initialize non-finites
23635 rt_InitInfAndNaN(sizeof(real_T));
23636
23637 {
23638 robotics_slmanip_internal__as_T *obj;
23639 b_inverseKinematics_cartesian_T *obj_0;
23640 h_robotics_core_internal_Damp_T *obj_1;
23641 static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
23642
23643 static const char_T tmp_0[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
23644 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
23645
23646 static const char_T tmp_1[5] = { '/', 'o', 'i', '_', 'x' };
23647
23648 static const char_T tmp_2[5] = { '/', 'o', 'i', '_', 'y' };
23649
23650 static const char_T tmp_3[5] = { '/', 'o', 'i', '_', 'z' };
23651
23652 static const char_T tmp_4[5] = { '/', 'o', 'i', '_', 'w' };
23653
23654 static const char_T tmp_5[5] = { '/', 'p', 'i', '_', 'x' };
23655
23656 static const char_T tmp_6[5] = { '/', 'p', 'i', '_', 'y' };
23657
23658 static const char_T tmp_7[5] = { '/', 'p', 'i', '_', 'z' };
23659
23660 static const char_T tmp_8[5] = { '/', 'o', 'f', '_', 'x' };
23661
23662 static const char_T tmp_9[5] = { '/', 'o', 'f', '_', 'y' };
23663
23664 static const char_T tmp_a[5] = { '/', 'o', 'f', '_', 'z' };
23665
23666 static const char_T tmp_b[5] = { '/', 'o', 'f', '_', 'w' };
23667
23668 static const char_T tmp_c[5] = { '/', 'p', 'f', '_', 'x' };
23669
23670 static const char_T tmp_d[5] = { '/', 'p', 'f', '_', 'y' };
23671
23672 static const char_T tmp_e[5] = { '/', 'p', 'f', '_', 'z' };
23673
23674 static const char_T tmp_f[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23675 'o', 'x' };
23676
23677 static const char_T tmp_g[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23678 'o', 'y' };
23679
23680 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23681 'o', 'z' };
23682
23683 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23684 'p', 'x' };
23685
23686 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23687 'p', 'y' };
23688
23689 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23690 'p', 'z' };
23691
23692 static const char_T tmp_l[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
23693 'e', 'l', 'a', 'y' };
23694
23695 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
23696 // SystemInitialize for Enabled SubSystem: '<S9>/Enabled Subsystem'
23697 // SystemInitialize for Outport: '<S13>/Out1'
23698 cartesian_trajectory_planner_B.In1 = cartesian_trajectory_planner_P.Out1_Y0;
23699
23700 // End of SystemInitialize for SubSystem: '<S9>/Enabled Subsystem'
23701
23702 // Start for MATLABSystem: '<S9>/SourceBlock'
23703 cartesian_trajectory_planner_DW.obj_gl.matlabCodegenIsDeleted = false;
23704 cartesian_trajectory_planner_DW.obj_gl.isInitialized = 1;
23705 for (cartesian_trajectory_planner_B.i_jb = 0;
23706 cartesian_trajectory_planner_B.i_jb < 6;
23707 cartesian_trajectory_planner_B.i_jb++) {
23708 // InitializeConditions for Delay: '<Root>/Delay'
23709 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.i_jb]
23710 =
23711 cartesian_trajectory_planner_P.Delay_InitialCondition[cartesian_trajectory_planner_B.i_jb];
23712
23713 // Start for MATLABSystem: '<S9>/SourceBlock'
23714 cartesian_trajectory_planner_B.cv3[cartesian_trajectory_planner_B.i_jb] =
23715 tmp[cartesian_trajectory_planner_B.i_jb];
23716 }
23717
23718 // Start for MATLABSystem: '<S9>/SourceBlock'
23719 cartesian_trajectory_planner_B.cv3[6] = '\x00';
23720 Sub_cartesian_trajectory_planner_280.createSubscriber
23721 (cartesian_trajectory_planner_B.cv3, 1);
23722 cartesian_trajectory_planner_DW.obj_gl.isSetupComplete = true;
23723
23724 // End of SystemInitialize for SubSystem: '<Root>/Subscribe'
23725
23726 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
23727 // Start for MATLABSystem: '<S8>/SinkBlock'
23728 cartesian_trajectory_planner_DW.obj_n.matlabCodegenIsDeleted = false;
23729 cartesian_trajectory_planner_DW.obj_n.isInitialized = 1;
23730 for (cartesian_trajectory_planner_B.i_jb = 0;
23731 cartesian_trajectory_planner_B.i_jb < 17;
23732 cartesian_trajectory_planner_B.i_jb++) {
23733 cartesian_trajectory_planner_B.cv[cartesian_trajectory_planner_B.i_jb] =
23734 tmp_0[cartesian_trajectory_planner_B.i_jb];
23735 }
23736
23737 cartesian_trajectory_planner_B.cv[17] = '\x00';
23738 Pub_cartesian_trajectory_planner_278.createPublisher
23739 (cartesian_trajectory_planner_B.cv, 1);
23740 cartesian_trajectory_planner_DW.obj_n.isSetupComplete = true;
23741
23742 // End of Start for MATLABSystem: '<S8>/SinkBlock'
23743 // End of SystemInitialize for SubSystem: '<Root>/Publish'
23744
23745 // Start for MATLABSystem: '<Root>/Get Parameter'
23746 cartesian_trajectory_planner_DW.obj_cu.matlabCodegenIsDeleted = false;
23747 cartesian_trajectory_planner_DW.obj_cu.isInitialized = 1;
23748 cartesian_trajectory_planner_B.cv5[0] = '/';
23749 cartesian_trajectory_planner_B.cv5[1] = 'V';
23750 cartesian_trajectory_planner_B.cv5[2] = 'e';
23751 cartesian_trajectory_planner_B.cv5[3] = 'e';
23752 cartesian_trajectory_planner_B.cv5[4] = '\x00';
23753 ParamGet_cartesian_trajectory_planner_271.initialize
23754 (cartesian_trajectory_planner_B.cv5);
23755 ParamGet_cartesian_trajectory_planner_271.initialize_error_codes(0, 1, 2, 3);
23756 ParamGet_cartesian_trajectory_planner_271.set_initial_value(1.0);
23757 cartesian_trajectory_planner_DW.obj_cu.isSetupComplete = true;
23758
23759 // Start for MATLABSystem: '<Root>/Get Parameter1'
23760 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = false;
23761 cartesian_trajectory_planner_DW.obj_k.isInitialized = 1;
23762 cartesian_trajectory_planner_B.cv5[0] = '/';
23763 cartesian_trajectory_planner_B.cv5[1] = 'A';
23764 cartesian_trajectory_planner_B.cv5[2] = 'e';
23765 cartesian_trajectory_planner_B.cv5[3] = 'e';
23766 cartesian_trajectory_planner_B.cv5[4] = '\x00';
23767 ParamGet_cartesian_trajectory_planner_272.initialize
23768 (cartesian_trajectory_planner_B.cv5);
23769 ParamGet_cartesian_trajectory_planner_272.initialize_error_codes(0, 1, 2, 3);
23770 ParamGet_cartesian_trajectory_planner_272.set_initial_value(0.1);
23771 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = true;
23772
23773 // Start for MATLABSystem: '<S14>/Get Parameter3'
23774 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = false;
23775 cartesian_trajectory_planner_DW.obj_es.isInitialized = 1;
23776 for (cartesian_trajectory_planner_B.i_jb = 0;
23777 cartesian_trajectory_planner_B.i_jb < 5;
23778 cartesian_trajectory_planner_B.i_jb++) {
23779 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23780 tmp_1[cartesian_trajectory_planner_B.i_jb];
23781 }
23782
23783 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23784 ParamGet_cartesian_trajectory_planner_291.initialize
23785 (cartesian_trajectory_planner_B.cv4);
23786 ParamGet_cartesian_trajectory_planner_291.initialize_error_codes(0, 1, 2, 3);
23787 ParamGet_cartesian_trajectory_planner_291.set_initial_value(0.0);
23788 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = true;
23789
23790 // End of Start for MATLABSystem: '<S14>/Get Parameter3'
23791
23792 // Start for MATLABSystem: '<S14>/Get Parameter4'
23793 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
23794 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 1;
23795 for (cartesian_trajectory_planner_B.i_jb = 0;
23796 cartesian_trajectory_planner_B.i_jb < 5;
23797 cartesian_trajectory_planner_B.i_jb++) {
23798 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23799 tmp_2[cartesian_trajectory_planner_B.i_jb];
23800 }
23801
23802 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23803 ParamGet_cartesian_trajectory_planner_292.initialize
23804 (cartesian_trajectory_planner_B.cv4);
23805 ParamGet_cartesian_trajectory_planner_292.initialize_error_codes(0, 1, 2, 3);
23806 ParamGet_cartesian_trajectory_planner_292.set_initial_value(0.0);
23807 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = true;
23808
23809 // End of Start for MATLABSystem: '<S14>/Get Parameter4'
23810
23811 // Start for MATLABSystem: '<S14>/Get Parameter5'
23812 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = false;
23813 cartesian_trajectory_planner_DW.obj_en.isInitialized = 1;
23814 for (cartesian_trajectory_planner_B.i_jb = 0;
23815 cartesian_trajectory_planner_B.i_jb < 5;
23816 cartesian_trajectory_planner_B.i_jb++) {
23817 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23818 tmp_3[cartesian_trajectory_planner_B.i_jb];
23819 }
23820
23821 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23822 ParamGet_cartesian_trajectory_planner_293.initialize
23823 (cartesian_trajectory_planner_B.cv4);
23824 ParamGet_cartesian_trajectory_planner_293.initialize_error_codes(0, 1, 2, 3);
23825 ParamGet_cartesian_trajectory_planner_293.set_initial_value(0.0);
23826 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = true;
23827
23828 // End of Start for MATLABSystem: '<S14>/Get Parameter5'
23829
23830 // Start for MATLABSystem: '<S14>/Get Parameter6'
23831 cartesian_trajectory_planner_DW.obj_l0.matlabCodegenIsDeleted = false;
23832 cartesian_trajectory_planner_DW.obj_l0.isInitialized = 1;
23833 for (cartesian_trajectory_planner_B.i_jb = 0;
23834 cartesian_trajectory_planner_B.i_jb < 5;
23835 cartesian_trajectory_planner_B.i_jb++) {
23836 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23837 tmp_4[cartesian_trajectory_planner_B.i_jb];
23838 }
23839
23840 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23841 ParamGet_cartesian_trajectory_planner_294.initialize
23842 (cartesian_trajectory_planner_B.cv4);
23843 ParamGet_cartesian_trajectory_planner_294.initialize_error_codes(0, 1, 2, 3);
23844 ParamGet_cartesian_trajectory_planner_294.set_initial_value(1.0);
23845 cartesian_trajectory_planner_DW.obj_l0.isSetupComplete = true;
23846
23847 // End of Start for MATLABSystem: '<S14>/Get Parameter6'
23848
23849 // Start for MATLABSystem: '<S14>/Get Parameter'
23850 cartesian_trajectory_planner_DW.obj_i.matlabCodegenIsDeleted = false;
23851 cartesian_trajectory_planner_DW.obj_i.isInitialized = 1;
23852 for (cartesian_trajectory_planner_B.i_jb = 0;
23853 cartesian_trajectory_planner_B.i_jb < 5;
23854 cartesian_trajectory_planner_B.i_jb++) {
23855 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23856 tmp_5[cartesian_trajectory_planner_B.i_jb];
23857 }
23858
23859 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23860 ParamGet_cartesian_trajectory_planner_288.initialize
23861 (cartesian_trajectory_planner_B.cv4);
23862 ParamGet_cartesian_trajectory_planner_288.initialize_error_codes(0, 1, 2, 3);
23863 ParamGet_cartesian_trajectory_planner_288.set_initial_value(0.0);
23864 cartesian_trajectory_planner_DW.obj_i.isSetupComplete = true;
23865
23866 // End of Start for MATLABSystem: '<S14>/Get Parameter'
23867
23868 // Start for MATLABSystem: '<S14>/Get Parameter1'
23869 cartesian_trajectory_planner_DW.obj_db.matlabCodegenIsDeleted = false;
23870 cartesian_trajectory_planner_DW.obj_db.isInitialized = 1;
23871 for (cartesian_trajectory_planner_B.i_jb = 0;
23872 cartesian_trajectory_planner_B.i_jb < 5;
23873 cartesian_trajectory_planner_B.i_jb++) {
23874 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23875 tmp_6[cartesian_trajectory_planner_B.i_jb];
23876 }
23877
23878 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23879 ParamGet_cartesian_trajectory_planner_289.initialize
23880 (cartesian_trajectory_planner_B.cv4);
23881 ParamGet_cartesian_trajectory_planner_289.initialize_error_codes(0, 1, 2, 3);
23882 ParamGet_cartesian_trajectory_planner_289.set_initial_value(0.0);
23883 cartesian_trajectory_planner_DW.obj_db.isSetupComplete = true;
23884
23885 // End of Start for MATLABSystem: '<S14>/Get Parameter1'
23886
23887 // Start for MATLABSystem: '<S14>/Get Parameter2'
23888 cartesian_trajectory_planner_DW.obj_oq.matlabCodegenIsDeleted = false;
23889 cartesian_trajectory_planner_DW.obj_oq.isInitialized = 1;
23890 for (cartesian_trajectory_planner_B.i_jb = 0;
23891 cartesian_trajectory_planner_B.i_jb < 5;
23892 cartesian_trajectory_planner_B.i_jb++) {
23893 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23894 tmp_7[cartesian_trajectory_planner_B.i_jb];
23895 }
23896
23897 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23898 ParamGet_cartesian_trajectory_planner_290.initialize
23899 (cartesian_trajectory_planner_B.cv4);
23900 ParamGet_cartesian_trajectory_planner_290.initialize_error_codes(0, 1, 2, 3);
23901 ParamGet_cartesian_trajectory_planner_290.set_initial_value(0.99);
23902 cartesian_trajectory_planner_DW.obj_oq.isSetupComplete = true;
23903
23904 // End of Start for MATLABSystem: '<S14>/Get Parameter2'
23905
23906 // Start for MATLABSystem: '<S15>/Get Parameter3'
23907 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
23908 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 1;
23909 for (cartesian_trajectory_planner_B.i_jb = 0;
23910 cartesian_trajectory_planner_B.i_jb < 5;
23911 cartesian_trajectory_planner_B.i_jb++) {
23912 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23913 tmp_8[cartesian_trajectory_planner_B.i_jb];
23914 }
23915
23916 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23917 ParamGet_cartesian_trajectory_planner_303.initialize
23918 (cartesian_trajectory_planner_B.cv4);
23919 ParamGet_cartesian_trajectory_planner_303.initialize_error_codes(0, 1, 2, 3);
23920 ParamGet_cartesian_trajectory_planner_303.set_initial_value(0.39);
23921 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = true;
23922
23923 // End of Start for MATLABSystem: '<S15>/Get Parameter3'
23924
23925 // Start for MATLABSystem: '<S15>/Get Parameter4'
23926 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = false;
23927 cartesian_trajectory_planner_DW.obj_a.isInitialized = 1;
23928 for (cartesian_trajectory_planner_B.i_jb = 0;
23929 cartesian_trajectory_planner_B.i_jb < 5;
23930 cartesian_trajectory_planner_B.i_jb++) {
23931 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23932 tmp_9[cartesian_trajectory_planner_B.i_jb];
23933 }
23934
23935 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23936 ParamGet_cartesian_trajectory_planner_304.initialize
23937 (cartesian_trajectory_planner_B.cv4);
23938 ParamGet_cartesian_trajectory_planner_304.initialize_error_codes(0, 1, 2, 3);
23939 ParamGet_cartesian_trajectory_planner_304.set_initial_value(0.89);
23940 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = true;
23941
23942 // End of Start for MATLABSystem: '<S15>/Get Parameter4'
23943
23944 // Start for MATLABSystem: '<S15>/Get Parameter5'
23945 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = false;
23946 cartesian_trajectory_planner_DW.obj_p.isInitialized = 1;
23947 for (cartesian_trajectory_planner_B.i_jb = 0;
23948 cartesian_trajectory_planner_B.i_jb < 5;
23949 cartesian_trajectory_planner_B.i_jb++) {
23950 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23951 tmp_a[cartesian_trajectory_planner_B.i_jb];
23952 }
23953
23954 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23955 ParamGet_cartesian_trajectory_planner_305.initialize
23956 (cartesian_trajectory_planner_B.cv4);
23957 ParamGet_cartesian_trajectory_planner_305.initialize_error_codes(0, 1, 2, 3);
23958 ParamGet_cartesian_trajectory_planner_305.set_initial_value(0.2);
23959 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = true;
23960
23961 // End of Start for MATLABSystem: '<S15>/Get Parameter5'
23962
23963 // Start for MATLABSystem: '<S15>/Get Parameter6'
23964 cartesian_trajectory_planner_DW.obj_l.matlabCodegenIsDeleted = false;
23965 cartesian_trajectory_planner_DW.obj_l.isInitialized = 1;
23966 for (cartesian_trajectory_planner_B.i_jb = 0;
23967 cartesian_trajectory_planner_B.i_jb < 5;
23968 cartesian_trajectory_planner_B.i_jb++) {
23969 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23970 tmp_b[cartesian_trajectory_planner_B.i_jb];
23971 }
23972
23973 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23974 ParamGet_cartesian_trajectory_planner_306.initialize
23975 (cartesian_trajectory_planner_B.cv4);
23976 ParamGet_cartesian_trajectory_planner_306.initialize_error_codes(0, 1, 2, 3);
23977 ParamGet_cartesian_trajectory_planner_306.set_initial_value(-0.12);
23978 cartesian_trajectory_planner_DW.obj_l.isSetupComplete = true;
23979
23980 // End of Start for MATLABSystem: '<S15>/Get Parameter6'
23981
23982 // Start for MATLABSystem: '<S15>/Get Parameter'
23983 cartesian_trajectory_planner_DW.obj_mb.matlabCodegenIsDeleted = false;
23984 cartesian_trajectory_planner_DW.obj_mb.isInitialized = 1;
23985 for (cartesian_trajectory_planner_B.i_jb = 0;
23986 cartesian_trajectory_planner_B.i_jb < 5;
23987 cartesian_trajectory_planner_B.i_jb++) {
23988 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
23989 tmp_c[cartesian_trajectory_planner_B.i_jb];
23990 }
23991
23992 cartesian_trajectory_planner_B.cv4[5] = '\x00';
23993 ParamGet_cartesian_trajectory_planner_300.initialize
23994 (cartesian_trajectory_planner_B.cv4);
23995 ParamGet_cartesian_trajectory_planner_300.initialize_error_codes(0, 1, 2, 3);
23996 ParamGet_cartesian_trajectory_planner_300.set_initial_value(0.3);
23997 cartesian_trajectory_planner_DW.obj_mb.isSetupComplete = true;
23998
23999 // End of Start for MATLABSystem: '<S15>/Get Parameter'
24000
24001 // Start for MATLABSystem: '<S15>/Get Parameter1'
24002 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = false;
24003 cartesian_trajectory_planner_DW.obj_c.isInitialized = 1;
24004 for (cartesian_trajectory_planner_B.i_jb = 0;
24005 cartesian_trajectory_planner_B.i_jb < 5;
24006 cartesian_trajectory_planner_B.i_jb++) {
24007 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
24008 tmp_d[cartesian_trajectory_planner_B.i_jb];
24009 }
24010
24011 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24012 ParamGet_cartesian_trajectory_planner_301.initialize
24013 (cartesian_trajectory_planner_B.cv4);
24014 ParamGet_cartesian_trajectory_planner_301.initialize_error_codes(0, 1, 2, 3);
24015 ParamGet_cartesian_trajectory_planner_301.set_initial_value(0.3);
24016 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = true;
24017
24018 // End of Start for MATLABSystem: '<S15>/Get Parameter1'
24019
24020 // Start for MATLABSystem: '<S15>/Get Parameter2'
24021 cartesian_trajectory_planner_DW.obj_f2.matlabCodegenIsDeleted = false;
24022 cartesian_trajectory_planner_DW.obj_f2.isInitialized = 1;
24023 for (cartesian_trajectory_planner_B.i_jb = 0;
24024 cartesian_trajectory_planner_B.i_jb < 5;
24025 cartesian_trajectory_planner_B.i_jb++) {
24026 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_jb] =
24027 tmp_e[cartesian_trajectory_planner_B.i_jb];
24028 }
24029
24030 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24031 ParamGet_cartesian_trajectory_planner_302.initialize
24032 (cartesian_trajectory_planner_B.cv4);
24033 ParamGet_cartesian_trajectory_planner_302.initialize_error_codes(0, 1, 2, 3);
24034 ParamGet_cartesian_trajectory_planner_302.set_initial_value(0.3);
24035 cartesian_trajectory_planner_DW.obj_f2.isSetupComplete = true;
24036
24037 // End of Start for MATLABSystem: '<S15>/Get Parameter2'
24038
24039 // Start for MATLABSystem: '<S11>/Get Parameter'
24040 cartesian_trajectory_planner_DW.obj_e.matlabCodegenIsDeleted = false;
24041 cartesian_trajectory_planner_DW.obj_e.isInitialized = 1;
24042 for (cartesian_trajectory_planner_B.i_jb = 0;
24043 cartesian_trajectory_planner_B.i_jb < 10;
24044 cartesian_trajectory_planner_B.i_jb++) {
24045 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24046 tmp_f[cartesian_trajectory_planner_B.i_jb];
24047 }
24048
24049 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24050 ParamGet_cartesian_trajectory_planner_312.initialize
24051 (cartesian_trajectory_planner_B.cv2);
24052 ParamGet_cartesian_trajectory_planner_312.initialize_error_codes(0, 1, 2, 3);
24053 ParamGet_cartesian_trajectory_planner_312.set_initial_value(0.0);
24054 cartesian_trajectory_planner_DW.obj_e.isSetupComplete = true;
24055
24056 // End of Start for MATLABSystem: '<S11>/Get Parameter'
24057
24058 // Start for MATLABSystem: '<S11>/Get Parameter1'
24059 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = false;
24060 cartesian_trajectory_planner_DW.obj_d.isInitialized = 1;
24061 for (cartesian_trajectory_planner_B.i_jb = 0;
24062 cartesian_trajectory_planner_B.i_jb < 10;
24063 cartesian_trajectory_planner_B.i_jb++) {
24064 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24065 tmp_g[cartesian_trajectory_planner_B.i_jb];
24066 }
24067
24068 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24069 ParamGet_cartesian_trajectory_planner_313.initialize
24070 (cartesian_trajectory_planner_B.cv2);
24071 ParamGet_cartesian_trajectory_planner_313.initialize_error_codes(0, 1, 2, 3);
24072 ParamGet_cartesian_trajectory_planner_313.set_initial_value(0.0);
24073 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = true;
24074
24075 // End of Start for MATLABSystem: '<S11>/Get Parameter1'
24076
24077 // Start for MATLABSystem: '<S11>/Get Parameter2'
24078 cartesian_trajectory_planner_DW.obj_m.matlabCodegenIsDeleted = false;
24079 cartesian_trajectory_planner_DW.obj_m.isInitialized = 1;
24080 for (cartesian_trajectory_planner_B.i_jb = 0;
24081 cartesian_trajectory_planner_B.i_jb < 10;
24082 cartesian_trajectory_planner_B.i_jb++) {
24083 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24084 tmp_h[cartesian_trajectory_planner_B.i_jb];
24085 }
24086
24087 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24088 ParamGet_cartesian_trajectory_planner_314.initialize
24089 (cartesian_trajectory_planner_B.cv2);
24090 ParamGet_cartesian_trajectory_planner_314.initialize_error_codes(0, 1, 2, 3);
24091 ParamGet_cartesian_trajectory_planner_314.set_initial_value(0.0);
24092 cartesian_trajectory_planner_DW.obj_m.isSetupComplete = true;
24093
24094 // End of Start for MATLABSystem: '<S11>/Get Parameter2'
24095
24096 // Start for MATLABSystem: '<S11>/Get Parameter3'
24097 cartesian_trajectory_planner_DW.obj_h.matlabCodegenIsDeleted = false;
24098 cartesian_trajectory_planner_DW.obj_h.isInitialized = 1;
24099 for (cartesian_trajectory_planner_B.i_jb = 0;
24100 cartesian_trajectory_planner_B.i_jb < 10;
24101 cartesian_trajectory_planner_B.i_jb++) {
24102 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24103 tmp_i[cartesian_trajectory_planner_B.i_jb];
24104 }
24105
24106 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24107 ParamGet_cartesian_trajectory_planner_315.initialize
24108 (cartesian_trajectory_planner_B.cv2);
24109 ParamGet_cartesian_trajectory_planner_315.initialize_error_codes(0, 1, 2, 3);
24110 ParamGet_cartesian_trajectory_planner_315.set_initial_value(1.0);
24111 cartesian_trajectory_planner_DW.obj_h.isSetupComplete = true;
24112
24113 // End of Start for MATLABSystem: '<S11>/Get Parameter3'
24114
24115 // Start for MATLABSystem: '<S11>/Get Parameter4'
24116 cartesian_trajectory_planner_DW.obj_mf.matlabCodegenIsDeleted = false;
24117 cartesian_trajectory_planner_DW.obj_mf.isInitialized = 1;
24118 for (cartesian_trajectory_planner_B.i_jb = 0;
24119 cartesian_trajectory_planner_B.i_jb < 10;
24120 cartesian_trajectory_planner_B.i_jb++) {
24121 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24122 tmp_j[cartesian_trajectory_planner_B.i_jb];
24123 }
24124
24125 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24126 ParamGet_cartesian_trajectory_planner_316.initialize
24127 (cartesian_trajectory_planner_B.cv2);
24128 ParamGet_cartesian_trajectory_planner_316.initialize_error_codes(0, 1, 2, 3);
24129 ParamGet_cartesian_trajectory_planner_316.set_initial_value(1.0);
24130 cartesian_trajectory_planner_DW.obj_mf.isSetupComplete = true;
24131
24132 // End of Start for MATLABSystem: '<S11>/Get Parameter4'
24133
24134 // Start for MATLABSystem: '<S11>/Get Parameter5'
24135 cartesian_trajectory_planner_DW.obj_o.matlabCodegenIsDeleted = false;
24136 cartesian_trajectory_planner_DW.obj_o.isInitialized = 1;
24137 for (cartesian_trajectory_planner_B.i_jb = 0;
24138 cartesian_trajectory_planner_B.i_jb < 10;
24139 cartesian_trajectory_planner_B.i_jb++) {
24140 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_jb] =
24141 tmp_k[cartesian_trajectory_planner_B.i_jb];
24142 }
24143
24144 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24145 ParamGet_cartesian_trajectory_planner_317.initialize
24146 (cartesian_trajectory_planner_B.cv2);
24147 ParamGet_cartesian_trajectory_planner_317.initialize_error_codes(0, 1, 2, 3);
24148 ParamGet_cartesian_trajectory_planner_317.set_initial_value(1.0);
24149 cartesian_trajectory_planner_DW.obj_o.isSetupComplete = true;
24150
24151 // End of Start for MATLABSystem: '<S11>/Get Parameter5'
24152 emxInitStruct_robotics_slmanip_(&cartesian_trajectory_planner_DW.obj);
24153 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_1);
24154 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_50);
24155 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_49);
24156 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_48);
24157 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_47);
24158 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_46);
24159 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_45);
24160 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_44);
24161 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_43);
24162 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_42);
24163 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_41);
24164 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_40);
24165 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_39);
24166 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_38);
24167 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_37);
24168 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_36);
24169 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_35);
24170 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_34);
24171 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_33);
24172 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_32);
24173 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_31);
24174 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_30);
24175 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_29);
24176 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_28);
24177 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_27);
24178 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_26);
24179 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_25);
24180 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_24);
24181 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_23);
24182 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_22);
24183 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_21);
24184 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_20);
24185 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_19);
24186 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_18);
24187 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_17);
24188 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_16);
24189 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_15);
24190 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_14);
24191 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_13);
24192 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_12);
24193 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_11);
24194 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_10);
24195 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_9);
24196 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_8);
24197 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_7);
24198 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_6);
24199 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_5);
24200 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_4);
24201 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_3);
24202 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_2);
24203 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
24204 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
24205 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
24206 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
24207 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
24208 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
24209 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
24210 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
24211 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
24212 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
24213 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
24214 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
24215 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
24216 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
24217 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
24218 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
24219 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
24220 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
24221 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
24222 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
24223 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
24224 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
24225 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
24226 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
24227 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
24228 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
24229 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
24230 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
24231 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
24232 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
24233 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
24234 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
24235 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
24236 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
24237 emxInitStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
24238 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
24239 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
24240 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
24241 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
24242 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
24243 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
24244 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
24245 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
24246 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
24247 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
24248 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
24249 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
24250 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
24251 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
24252 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
24253 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
24254 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
24255 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
24256
24257 // Start for MATLABSystem: '<S5>/MATLAB System'
24258 cartesian_trajectory_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;
24259 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = true;
24260 cartesian_tr_eml_rand_mt19937ar(cartesian_trajectory_planner_DW.state_m);
24261 cartesian_trajectory_planner_DW.obj.isInitialized = 0;
24262 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = false;
24263 obj = &cartesian_trajectory_planner_DW.obj;
24264 cartesian_trajectory_planner_DW.obj.isInitialized = 1;
24265 RigidBodyTree_RigidBodyTree_as
24266 (&cartesian_trajectory_planner_DW.obj.TreeInternal,
24267 &cartesian_trajectory_planner_DW.gobj_90,
24268 &cartesian_trajectory_planner_DW.gobj_91,
24269 &cartesian_trajectory_planner_DW.gobj_92,
24270 &cartesian_trajectory_planner_DW.gobj_93,
24271 &cartesian_trajectory_planner_DW.gobj_94,
24272 &cartesian_trajectory_planner_DW.gobj_95,
24273 &cartesian_trajectory_planner_DW.gobj_96,
24274 &cartesian_trajectory_planner_DW.gobj_89);
24275 obj_0 = &cartesian_trajectory_planner_DW.obj.IKInternal;
24276 obj_0->isInitialized = 0;
24277 inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,
24278 &cartesian_trajectory_planner_DW.gobj_69,
24279 &cartesian_trajectory_planner_DW.gobj_70,
24280 &cartesian_trajectory_planner_DW.gobj_71,
24281 &cartesian_trajectory_planner_DW.gobj_72,
24282 &cartesian_trajectory_planner_DW.gobj_73,
24283 &cartesian_trajectory_planner_DW.gobj_74,
24284 &cartesian_trajectory_planner_DW.gobj_75,
24285 &cartesian_trajectory_planner_DW.gobj_76,
24286 &cartesian_trajectory_planner_DW.gobj_77,
24287 &cartesian_trajectory_planner_DW.gobj_78,
24288 &cartesian_trajectory_planner_DW.gobj_79,
24289 &cartesian_trajectory_planner_DW.gobj_80,
24290 &cartesian_trajectory_planner_DW.gobj_81,
24291 &cartesian_trajectory_planner_DW.gobj_82,
24292 &cartesian_trajectory_planner_DW.gobj_51,
24293 &cartesian_trajectory_planner_DW.gobj_28,
24294 &cartesian_trajectory_planner_DW.gobj_29,
24295 &cartesian_trajectory_planner_DW.gobj_30,
24296 &cartesian_trajectory_planner_DW.gobj_31,
24297 &cartesian_trajectory_planner_DW.gobj_32,
24298 &cartesian_trajectory_planner_DW.gobj_33,
24299 &cartesian_trajectory_planner_DW.gobj_34,
24300 &cartesian_trajectory_planner_DW.gobj_35,
24301 &cartesian_trajectory_planner_DW.gobj_36,
24302 &cartesian_trajectory_planner_DW.gobj_37,
24303 &cartesian_trajectory_planner_DW.gobj_38,
24304 &cartesian_trajectory_planner_DW.gobj_39,
24305 &cartesian_trajectory_planner_DW.gobj_40,
24306 &cartesian_trajectory_planner_DW.gobj_41,
24307 &cartesian_trajectory_planner_DW.gobj_42,
24308 &cartesian_trajectory_planner_DW.gobj_43,
24309 &cartesian_trajectory_planner_DW.gobj_44,
24310 &cartesian_trajectory_planner_DW.gobj_45,
24311 &cartesian_trajectory_planner_DW.gobj_46,
24312 &cartesian_trajectory_planner_DW.gobj_47,
24313 &cartesian_trajectory_planner_DW.gobj_48,
24314 &cartesian_trajectory_planner_DW.gobj_49,
24315 &cartesian_trajectory_planner_DW.gobj_50,
24316 &cartesian_trajectory_planner_DW.gobj_1,
24317 &cartesian_trajectory_planner_DW.gobj_27,
24318 &cartesian_trajectory_planner_DW.gobj_68,
24319 &cartesian_trajectory_planner_DW.gobj_83);
24320 obj_0->Solver = DampedBFGSwGradientProjection_D
24321 (&cartesian_trajectory_planner_DW.gobj_87);
24322 obj_1 = obj_0->Solver;
24323 obj_1->MaxNumIteration = 1500.0;
24324 obj_1->MaxTime = 10.0;
24325 obj_1->GradientTolerance = 1.0E-7;
24326 obj_1->SolutionTolerance = 1.0E-6;
24327 obj_1->ConstraintsOn = true;
24328 obj_1->RandomRestart = false;
24329 obj_1->StepTolerance = 1.0E-14;
24330 obj_0->matlabCodegenIsDeleted = false;
24331 emxInitStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj_g);
24332 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_1_h);
24333 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_16_c);
24334 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_15_g);
24335 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_14_h);
24336 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_13_a);
24337 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_12_b);
24338 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_11_i);
24339 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_10_f);
24340 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_9_g);
24341 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_8_d);
24342 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_7_b);
24343 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_6_a);
24344 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_5_g);
24345 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_4_j);
24346 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_3_l);
24347 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_2_m);
24348
24349 // Start for MATLABSystem: '<S3>/MATLAB System'
24350 cartesian_trajectory_planner_DW.obj_g.isInitialized = 0;
24351 cartesian_trajectory_planner_DW.obj_g.isInitialized = 1;
24352 car_RigidBodyTree_RigidBodyTree
24353 (&cartesian_trajectory_planner_DW.obj_g.TreeInternal,
24354 &cartesian_trajectory_planner_DW.gobj_2_m,
24355 &cartesian_trajectory_planner_DW.gobj_4_j,
24356 &cartesian_trajectory_planner_DW.gobj_5_g,
24357 &cartesian_trajectory_planner_DW.gobj_6_a,
24358 &cartesian_trajectory_planner_DW.gobj_7_b,
24359 &cartesian_trajectory_planner_DW.gobj_8_d,
24360 &cartesian_trajectory_planner_DW.gobj_9_g,
24361 &cartesian_trajectory_planner_DW.gobj_3_l);
24362 emxInitStruct_robotics_slman_as(&cartesian_trajectory_planner_DW.obj_f);
24363 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_1_f);
24364 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_16_k);
24365 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_15_l);
24366 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_14_hu);
24367 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_13_p);
24368 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_12_e);
24369 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_11_c);
24370 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_10_n);
24371 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_9_l);
24372 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_8_l);
24373 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_7_n);
24374 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_6_j);
24375 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_5_n);
24376 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_4_f);
24377 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_3_i);
24378 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_2_a);
24379
24380 // Start for MATLABSystem: '<S4>/MATLAB System'
24381 cartesian_trajectory_planner_DW.obj_f.isInitialized = 0;
24382 cartesian_trajectory_planner_DW.obj_f.isInitialized = 1;
24383 c_RigidBodyTree_RigidBodyTree_a
24384 (&cartesian_trajectory_planner_DW.obj_f.TreeInternal,
24385 &cartesian_trajectory_planner_DW.gobj_2_a,
24386 &cartesian_trajectory_planner_DW.gobj_4_f,
24387 &cartesian_trajectory_planner_DW.gobj_5_n,
24388 &cartesian_trajectory_planner_DW.gobj_6_j,
24389 &cartesian_trajectory_planner_DW.gobj_7_n,
24390 &cartesian_trajectory_planner_DW.gobj_8_l,
24391 &cartesian_trajectory_planner_DW.gobj_9_l,
24392 &cartesian_trajectory_planner_DW.gobj_3_i);
24393
24394 // Start for MATLABSystem: '<S10>/Get Parameter'
24395 cartesian_trajectory_planner_DW.obj_di.matlabCodegenIsDeleted = false;
24396 cartesian_trajectory_planner_DW.obj_di.isInitialized = 1;
24397 for (cartesian_trajectory_planner_B.i_jb = 0;
24398 cartesian_trajectory_planner_B.i_jb < 12;
24399 cartesian_trajectory_planner_B.i_jb++) {
24400 cartesian_trajectory_planner_B.cv1[cartesian_trajectory_planner_B.i_jb] =
24401 tmp_l[cartesian_trajectory_planner_B.i_jb];
24402 }
24403
24404 cartesian_trajectory_planner_B.cv1[12] = '\x00';
24405 ParamGet_cartesian_trajectory_planner_283.initialize
24406 (cartesian_trajectory_planner_B.cv1);
24407 ParamGet_cartesian_trajectory_planner_283.initialize_error_codes(0, 1, 2, 3);
24408 ParamGet_cartesian_trajectory_planner_283.set_initial_value(40.0);
24409 cartesian_trajectory_planner_DW.obj_di.isSetupComplete = true;
24410
24411 // End of Start for MATLABSystem: '<S10>/Get Parameter'
24412
24413 // Start for MATLABSystem: '<S10>/Get Parameter1'
24414 cartesian_trajectory_planner_DW.obj_cj.matlabCodegenIsDeleted = false;
24415 cartesian_trajectory_planner_DW.obj_cj.isInitialized = 1;
24416 cartesian_trajectory_planner_B.cv6[0] = '/';
24417 cartesian_trajectory_planner_B.cv6[1] = 't';
24418 cartesian_trajectory_planner_B.cv6[2] = 'f';
24419 cartesian_trajectory_planner_B.cv6[3] = '\x00';
24420 ParamGet_cartesian_trajectory_planner_284.initialize
24421 (cartesian_trajectory_planner_B.cv6);
24422 ParamGet_cartesian_trajectory_planner_284.initialize_error_codes(0, 1, 2, 3);
24423 ParamGet_cartesian_trajectory_planner_284.set_initial_value(70.0);
24424 cartesian_trajectory_planner_DW.obj_cj.isSetupComplete = true;
24425 }
24426}
24427
24428// Model terminate function
24429void cartesian_trajectory_planner_terminate(void)
24430{
24431 // Terminate for Atomic SubSystem: '<Root>/Subscribe'
24432 // Terminate for MATLABSystem: '<S9>/SourceBlock'
24433 matlabCodegenHandle_matla_astwh(&cartesian_trajectory_planner_DW.obj_gl);
24434
24435 // End of Terminate for SubSystem: '<Root>/Subscribe'
24436
24437 // Terminate for MATLABSystem: '<Root>/Get Parameter'
24438 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_cu);
24439
24440 // Terminate for MATLABSystem: '<Root>/Get Parameter1'
24441 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_k);
24442
24443 // Terminate for MATLABSystem: '<S14>/Get Parameter3'
24444 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_es);
24445
24446 // Terminate for MATLABSystem: '<S14>/Get Parameter4'
24447 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_f4);
24448
24449 // Terminate for MATLABSystem: '<S14>/Get Parameter5'
24450 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_en);
24451
24452 // Terminate for MATLABSystem: '<S14>/Get Parameter6'
24453 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_l0);
24454
24455 // Terminate for MATLABSystem: '<S14>/Get Parameter'
24456 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_i);
24457
24458 // Terminate for MATLABSystem: '<S14>/Get Parameter1'
24459 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_db);
24460
24461 // Terminate for MATLABSystem: '<S14>/Get Parameter2'
24462 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_oq);
24463
24464 // Terminate for MATLABSystem: '<S15>/Get Parameter3'
24465 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_e0);
24466
24467 // Terminate for MATLABSystem: '<S15>/Get Parameter4'
24468 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_a);
24469
24470 // Terminate for MATLABSystem: '<S15>/Get Parameter5'
24471 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_p);
24472
24473 // Terminate for MATLABSystem: '<S15>/Get Parameter6'
24474 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_l);
24475
24476 // Terminate for MATLABSystem: '<S15>/Get Parameter'
24477 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_mb);
24478
24479 // Terminate for MATLABSystem: '<S15>/Get Parameter1'
24480 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_c);
24481
24482 // Terminate for MATLABSystem: '<S15>/Get Parameter2'
24483 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_f2);
24484
24485 // Terminate for MATLABSystem: '<S11>/Get Parameter'
24486 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_e);
24487
24488 // Terminate for MATLABSystem: '<S11>/Get Parameter1'
24489 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_d);
24490
24491 // Terminate for MATLABSystem: '<S11>/Get Parameter2'
24492 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_m);
24493
24494 // Terminate for MATLABSystem: '<S11>/Get Parameter3'
24495 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_h);
24496
24497 // Terminate for MATLABSystem: '<S11>/Get Parameter4'
24498 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_mf);
24499
24500 // Terminate for MATLABSystem: '<S11>/Get Parameter5'
24501 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_o);
24502
24503 // Terminate for MATLABSystem: '<S5>/MATLAB System'
24504 matlabCodegenHandle_matlabC_ast(&cartesian_trajectory_planner_DW.obj);
24505 matlabCodegenHandle_matlabCo_as
24506 (&cartesian_trajectory_planner_DW.obj.IKInternal);
24507 emxFreeStruct_robotics_slmanip_(&cartesian_trajectory_planner_DW.obj);
24508 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_1);
24509 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_50);
24510 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_49);
24511 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_48);
24512 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_47);
24513 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_46);
24514 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_45);
24515 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_44);
24516 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_43);
24517 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_42);
24518 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_41);
24519 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_40);
24520 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_39);
24521 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_38);
24522 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_37);
24523 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_36);
24524 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_35);
24525 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_34);
24526 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_33);
24527 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_32);
24528 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_31);
24529 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_30);
24530 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_29);
24531 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_28);
24532 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_27);
24533 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_26);
24534 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_25);
24535 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_24);
24536 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_23);
24537 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_22);
24538 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_21);
24539 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_20);
24540 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_19);
24541 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_18);
24542 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_17);
24543 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_16);
24544 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_15);
24545 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_14);
24546 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_13);
24547 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_12);
24548 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_11);
24549 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_10);
24550 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_9);
24551 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_8);
24552 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_7);
24553 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_6);
24554 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_5);
24555 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_4);
24556 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_3);
24557 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_2);
24558 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
24559 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
24560 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
24561 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
24562 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
24563 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
24564 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
24565 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
24566 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
24567 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
24568 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
24569 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
24570 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
24571 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
24572 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
24573 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
24574 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
24575 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
24576 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
24577 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
24578 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
24579 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
24580 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
24581 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
24582 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
24583 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
24584 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
24585 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
24586 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
24587 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
24588 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
24589 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
24590 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
24591 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
24592 emxFreeStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
24593 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
24594 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
24595 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
24596 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
24597 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
24598 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
24599 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
24600 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
24601 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
24602 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
24603 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
24604 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
24605 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
24606 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
24607 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
24608 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
24609 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
24610 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
24611 emxFreeStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj_g);
24612 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_1_h);
24613 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_16_c);
24614 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_15_g);
24615 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_14_h);
24616 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_13_a);
24617 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_12_b);
24618 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_11_i);
24619 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_10_f);
24620 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_9_g);
24621 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_8_d);
24622 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_7_b);
24623 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_6_a);
24624 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_5_g);
24625 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_4_j);
24626 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_3_l);
24627 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_2_m);
24628 emxFreeStruct_robotics_slman_as(&cartesian_trajectory_planner_DW.obj_f);
24629 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_1_f);
24630 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_16_k);
24631 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_15_l);
24632 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_14_hu);
24633 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_13_p);
24634 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_12_e);
24635 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_11_c);
24636 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_10_n);
24637 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_9_l);
24638 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_8_l);
24639 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_7_n);
24640 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_6_j);
24641 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_5_n);
24642 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_4_f);
24643 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_3_i);
24644 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_2_a);
24645
24646 // Terminate for Atomic SubSystem: '<Root>/Publish'
24647 // Terminate for MATLABSystem: '<S8>/SinkBlock'
24648 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_n);
24649
24650 // End of Terminate for SubSystem: '<Root>/Publish'
24651
24652 // Terminate for MATLABSystem: '<S10>/Get Parameter'
24653 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_di);
24654
24655 // Terminate for MATLABSystem: '<S10>/Get Parameter1'
24656 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_cj);
24657}
24658
24659//
24660// File trailer for generated code.
24661//
24662// [EOF]
24663//
24664